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Abstract 

A new derivation of an algorithm which fuses the outputs of two 
Kalman filters is presented within the context of previous research in 
this field. Unlike works from different authors, this derivation clearly 
shows the combination of estimates to be optimal, minimizing the 
trace of the fused covariance matrix. The algorithm assumes that the 
filters use identical models, and are stable and operating optimally 
with respect to their own local measurements. Evidence is presented 
which indicates that the error ellipsoid derived from the covariance 
of the optimally fused estimate is contained within the intersections 
of the error ellipsoids of the two filters being fused. Modifications 
which reduce the algorithm’s data transmission requirements are also 
presented, including a scalar gain approximation, a cross-covariance 
update formula which employs only the two contributing filters’ auto- 
covariances, and a form of the algorithm which can be used to reini- 
tialize the two Kalman filters. A sufficient condition for using the 
optimally fused estimates to periodically reinitialize the Kalman fil- 
ters in this fashion is presented and proved as a theorem. When these 
results are applied to an optimal spacecraft rendezvous problem, sim- 
ulated performance results indicate that the use of optimally fused 
data leads to significantly improved robustness to initial target vehi- 
cle state errors. Two other applications of estimate fusion methods 
to spacecraft rendezvous are also described: state vector differencing, 
and redundancy management. 
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Summary 


Data fusion is a broad heading describing techniques for combining the information from various sensors In 
traditional spacecraft on-board navigation systems, data fusion is accomplished with a Kalman filter A basic 
assumption of the Kalman filter is that the noise associated with the signal from any one of the various sensors 
is uncorrelated from one time instant to the next, i.e. that the noise is “white.” Modern sensor systems, such 
as a Global Positioning System (GPS) receiver/processor or an integrated Inertial Navigation System / Global 
Positioning System (INS/GPS), perform significant manipulation of their input, or measurement, data such that 
the noise associated with their outputs, or states, cannot be viewed as white. Therefore, a Kalman filter cannot 
be successfully used to fuse the data from such systems without some sort of ad hoc work-around procedure 

Various authors have proposed solutions to this problem in various contexts. A solution minimizing the 
computation and data transmisssion requirements for a generic distributed network of linear quadratic estimators 
and controllers was proposed by Speyer [2] in 1979. During the eighties, Bar-Shalom and several colleagues 
presented results pertaining to data fusion in a multitarget, multisensor environment reminiscent of fire-control 
applications [5], [6], [7], More recently, Carlson has proposed a federated filtering approach [24], Other studies 
in this field have also been presented, and are discussed further in the sequel. In this report, a solution to the 
problem of fusing two Kalman filters operating in parallel is presented in the context of spacecraft navigation. 
In the approach presented here, the outputs, or state estimates, of the two filters are combined using weights 
based on the filters’ covariance matrices as well as the cross-covariance accounting for any correlation between 
the filters. These covariances account for the nonwhite nature of the noise associated with the filters’ estimates. 
The approach taken here has been called estimate fusion by the present author to distinguish it from other 
solution methods to the data fusion problem. 

In estimate fusion, only the states common to both filters are fused. However, correlations between these 
states and states unique to each filter are estimated and can be fed back to the filters in a periodic reinitialization 
procedure. Although computing the optimal weighting matrices for estimate fusion requires a matrix inversion 
(which can be time consuming for real-time flight software systems), the fact that only common states are fused 
means that the dimension of the matrix to be inverted is limited to the dimension of the common states. For 
typical spacecraft navigation systems, the only common states are position and velocity so, typically, a 6x6 
inverse will be required. With modern flight computers, computing this inverse is not an obstacle since the 
estimate fusion should typically be scheduled at a slower rate than the filters’ execution, and could possibly 
even be run as a background job. Alternatives for further reductions in computational requirements, including 
a scalar gain formulation, are presented in this report, although typically a sacrifice in performance is exacted 
for these suboptimal alternatives. No other significant issues associated with using estimate fusion in real-time 
systems are known to this author. 

This report is based on work I did while attending the University of Texas at Austin (U T.) on a JSC 
Graduate Fellowship during the 1991-92 academic year, and on subsequent work performed during a leave 
without pay for the 1992-93 academic year. The body of the report contains my thesis, which fulfilled part of 
the requirements for the Master’s degree I received from U.T. in December of 1992. In the thesis, the problem of 
fusing two optimally functioning, stable filters, which have identical states but different measurements, is solved. 
Suboptimal alternatives are derived with the consequences of their use explored, and a spacecraft rendezvous 
scenario is studied. The estimate fusion technique is found to combine, in a complementary way, the accuracies 
of a filter with relative state measurements and a filter with inertial state measurements. These results have 
obvious implications for the Space Shuttle since a GPS filter will be functioning alongside the existing rendezvous 
filter during rendezvous missions in the near future. Note that some minor typographical errors which appeared 
in the original work have been corrected. 

In addition to a revised version of my thesis, two appendices are included, each containing a technical paper 
which I prepared with assistance from my thesis advisor. Dr. Robert H. Bishop. Appendix A contains a 



paper which was presented at the 1993 American Institute of Aeronautics and Astronautics ( AIAA) Guidance. 
Navigation, and Control (GNC) Conference 1993 held in August of 1993 in Monterey, California. This paper 
summarizes the thesis and presents a few new results. Appendix B contains a paper which Dr. Bishop and I 
will present at the 1994 AIAA GNC Conference. This paper addresses the problem of fusing two filters with 
noncommon states , which is by fair the typical situation. Also addressed is the problem of what the rate of 
reinitialization of the filters with the fused estimates should be if it is desired to reinitialize the filters. This 
paper forms the foundation for our ongoing research in this area. 



Section 1 


Introduction 


An area of increasing interest in systems research is that of combining data from a distributed network of local 
sensors and/or estimators into a global estimate which combines the information available to each system in a 
complementary fashion. Such techniques have a wide range of applications, including distributed process control, 
fire control, remote sensing, and managing data from redundant systems. Another interest in data fusion research 
is motivated by the proliferation of black-box navigation systems such as most Global Positioning System (GPS) 
receivers. A desire of contemporary spacecraft designers is to combine such off-the-shelf systems in a distributed 
architecture in such a way that the measurement availability and geometry of the various systems complement 
one another in some optimal fashion. Typically, modification of the outputs of these systems to meet data 
fusion requirements is not a cost-effective option, so the desire exists to combine the available information into 
a globally optimal estimate, which may then be used to reset the local processors. In this way, modifications are 
made outside the existing system rather than inside. Such a scenario is the focus of the present work. 

First, new algorithms to perform the fusion function are developed and related to previous work. These 
algorithms are then illustrated through the use of a simple example. Finally, a system which optimally combines 
the state estimates of two Kalman filters on board a lunar orbiting spacecraft is considered. The spacecraft state 
estimates achieved using the fusion reset method are used to facilitate a rendezvous mission with another lunar 
orbiter. Through its application to this simulated lunar rendezvous mission, the efficacy of the estimate fusion 
technique is evaluated. 

1.1 Review of Approaches to Data Fusion 

Several approaches to the problem of data fusion are possible, and many have been considered in the literature. 
The most basic form of data fusion occurs in the globally optimal Kalman filter, which optimally combines raw 
measurement data from various sources. A basic assumption, however, is that the measurements are time-wise 
uncorrelated. If rather than raw measurements it is desired to optimally combine the estimates from several 
Kalman filters, the zero autocorrelation assumption will be violated. This problem has been referred to as filter 
cascading. Estimate fusion, as presented here, represents a solution to this problem. Other possibilities exist, 
such as the case in which some data sources are raw measurements and others are the outputs of estimation 
schemes. This class of data fusion has been classified as hybrid fusion. As mentioned previously, the present 
work is primarily concerned with a form of estimate fusion in which the estimates from two Kalman filters are 
combined to form an optimally fused estimate which is then used to reset these two filters. Some work of past 
researchers in this field will be reviewed to shed more light on these various approaches. While this review is by 
no means exhaustive, it attempts to consider many of the most significant works in the area. 

One of the earliest researches into estimate fusion was that of Willner, et al. [1]. In this work, the authors 
considered a problem they referred to as estimate compression. They showed how to calculate a weighted least 
squares global estimate using an arbitrary number of estimates from local processors as data. A restriction on 
this work was that all estimates had to be expressed in the same coordinate frame. This work requires the 
calculation of [jV x (.V - l)]/2 cross-covariance matrices of order n x n, where N is the number of estimates 
being combined, and n is the state dimension of each. In addition, this method also requires the calculation of 
the inverse of an njV x nN covariance matrix that corresponds to a state vector which contains all of the local 

st<ite vectors 

Later, a significant contribution was made by Speyer [2] in his solution to the discrete and continuous forms of 
the decentralized linear quadratic Gaussian control problem In this work, the author minimized the requirements 
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for data transmission between nodes in a decentralized network. Each node is required only to transmit to its 
neighbors a data vector, which has only the dimension of the control vector (if only the estimation problem is 
being solved, then a data vector having the same dimension as the state vector is transmitted). This method 
offered significant advantages over the work mentioned previously, because here the computations were divided 
among each of the local processors. The penalty for the reduced transmission requirements are the additional 
ralculations required to generate the data vector. An assumption of this work was that identical models were 
assumed for each of the local processors. 

The assumption of identical local models was addressed in the work of Willsky, et al. [3]. These researchers 
generalized the work of Speyer by giving necessary and sufficient conditions for estimating a global state from 
local estimates of arbitrary dimension expressed in arbitrary coordinate frames. In essence, this condition stated 
that any assumptions about relationships in the linear mapping of the states onto the measurements in the local 
models must be preserved in the global model. A result is that this “condition does not require that there be 
any physical relationship between the local states. ..and the global state....' 1 In addition to this general case, the 
authors examined subcases in which the local models are identical to and subsets of the global model. They also 
examined the smoothing problem. 

Willsky, et al. predicted that their work could be simplified. One such simplification can be found in the 
work of Alouani and Bird well [4]. These authors applied their solution to the nonlinear estimation problem to 
the linear data fusion problem and gave two theorems for its solution. The first is a theorem for updating the 
conditional densities of a system of arbitrary estimators. This theorem applied to Gauss-Markov processes leads 
to a second theorem which gives an algorithm for updating the mean and covariance of the global state estimate. 

The works above consider the problem of fusing an arbitrary number of estimates from local processors 
into a centralized global estimate. A related problem is whether or not two estimates which are to be combined 
actually originate from the same tracked object. This problem is known as data association and has been studied 
extensively by Bar-Shalam and others [5], [6], [7]. Data association requires testing the hypothesis that, within 
some tolerance level, two estimate tracks originate from the same target. Testing this hypothesis requires explicit 
use of the cross- covariance between the estimates, which are only used implicitly in Willner, et al. [1], Speyer 
[2], Willsky, et al. [3], and Alouani and Birdwell [4]. Bar-Shalom gives a dynamic algorithm for calculating the 
cross-covariance [5] and develops an estimate fusion algorithm which explicitly contains the cross-covariance [6]. 
Bar- Shalom's cross-covariance algorithm is derived by analogy to the minimum variance estimator. These ideas 
are tied together with the hypothesis testing algorithm in his book [7]. An important assumption made in his 
work is that the estimates being fused use identical models. 

A modification of the algorithm given by Bar-Shalom appears in a work by Blackman [8]. This approach is 
a hybrid version of data fusion where one track is assumed to be generated by directly processing measurement 
data in the manner of a Kalman filter. The track which is to be fused with this track consists of estimates 
output from a sensor which has done its own processing of its raw observations. Thus, inputs to the data fusion 
algorithm are both raw measurements and processed estimates. 

Another result similar to the algorithm derived by Bar-Shalom was independently developed by Bishop [9] 
using an optimization- based approach. The objective of this work is an optimal combination of the estimates 
from two Kalman filters which use identical models and are stable and operating optimally with respect to their 
local measurement sets. Bishop also proposes to reset each of the Kalman filters with the estimate achieved 
through this optimal combination. This derivation forms the starting point for the work in this thesis and will 
be covered in extensive detail in the sequel. 

1.2 The Utility of Estimate Fusion in a Rendezvous Problem 

The rendezvous navigation problem was chosen as an illustration of the efficacy of estimate fusion because it 
typically is performed using only measurements of the relative state of the two vehicles concerned. While this 
is of primary importance for the rendezvous targeting guidance algorithms, the absence of accurate inertial 
information can lead to problems [10]. Since most spacecraft also carry an inertial navigation system, a scheme 
for producing optimal combinations of the relative state and inertial state estimators would be a desirable 
achievement. 
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Section 2 


Fusion of Estimators Using Identical Models 


This section presents the algorithm used for estimate fusion in this work, demonstrating the basic features of the 
algorithm, including the equivalence of the optimal fused state vector to a global estimator and the role played in 
estimate fusion by the cross-covariance between the two estimators. Several extensions to the algorithm are also 
discussed, including an alternate update formula for the cross-covariance and a suboptimal form of the fusion 
gain. 


2.1 Derivation of the Optimal Combination 

The derivation of the optimal fusion algorithm is based on a cost function defined as the trace of the covariance 
associated with the fused estimate. This approach, derived originally by Bishop in reference [9], appears to be 
unique in the literature. 

To combine the estimates from two filters in some optimal fashion, a form for the optimal posterior estimate 
of the global state is assumed: 

Xop, = (/ - W)±x + Wxi, (2.1) 

where denotes an estimate, and x x and x 2 are state estimates from the individual filters. The fusion gain 
matrix, W , is to be determined. The a posteriori estimation error is defined as 

e, = x - x, i = 1, 2, opt . (2 2) 


From eq. 2.1 and eq. 2.2 , it follows that 

= (/ - W)e x + We 7 . (2.3) 

The state error covariance matrix for the optimal combination is defined to be 

Port = E[&or, 6*,]. (2 4) 

Computing P opt in eq. 2.4 using eopi in eq. 2.3 yields 

Pop, = A - (A “ RnW T - W(P X - flfj) (2 5) 

+W ( A + A- A 2 -«n)W rT . 

where A = £[ei ef]> A — £[®2 * 2 "]) and flu = fl[ei ej]. 

In eq. 2.5 . flu is the a posteriori cross-covariance matrix. Note that since both individual estimators are 

Kalman filters, we have the state update equation 

Xi = X* + Ki{yt - * = 1,2 

where ' ~ ' denotes estimates prior to the filter updates and y< — HiX + e hence, 

x, =Xi + K,Hi{xi -Xi) + Kit i, * = 1,2 


The estimation errors are given by 

e, = e, — K,H,e, - K t c ., *=1,2 
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where e, = x - x, . i = 1.2. Thus. 

E[ex ej] = £[{§i - K\H\ei - h\e L }{e 2 - A’ 2 // 2 e 2 - A' 2 £ 2 } r ] 

= E((I- A'i //i)e t ej(/ - K 2 H 2 ) T - (/ - A|ff|)S t c ?7\ 2 T 

-Ate i§[(/ - K 2 H 2 ) t + Ate tc [ A[ ] 

= (/- AiAi)Ai 2 (/- K 2 H 2 ) t , 

where R i2 has been propagated from the last update interval and A[et e J]. E[c t e?], and E[c , c have been 
assumed to be zero. Bar-Shalom [5] has shown that this propagation should be the same as that used for each 
filter's (auto-) covariance matrix, under the assumptions that both filters use the same covariance propagation 
method, and that this method accurately represents the propagation of the true state error covariance matrix. 
We will now choose an optimal W by minimizing the trace of P opt , that is 


minj — min trP opt . 

The following properties of the trace operator are useful in the subsequent derivation: 

dtrAB T dtrBA T 


dB 

dtrBAB T 


dB 


— *4, 


dB 


= 2BA, if A is symmetric. 


Taking the partial derivative of J with respect to W yields 


dJ 

dW 


= 2W(P r + P 2 - R lt - flf 2 ) - 2 (A - R l2 ). 


/) J 

The optimal W is then found by setting to zero 

dJ 

dW 


= 0, 


Wo, 


and solving for W as follows: 


,T\-i 


- (Pi - rt|j)(Pi + P 2 - R l2 - RJ 2 ) 


( 2 . 6 ) 


A great deal of simplification for Popt, given in eq. 2.5 , results if eq. 2.6 is used in eq. 2.5: 

Pop, = Pi- (Pi - Rn)(Pi + Pi- Rii - Rii)~ T (Pi - Riif 
- (Pi - Rl 7 )(Pl + Pl- Rl 2 - RZi)- l (Pi - R t ii) 

+ (Pi - Rn)(Pi + Pi - Rii - R t h)~\Pi + Pi- R11 - Rfi) 
x (P1 + P2- Rii-Rli)- T (Pi-Rii) T - 


Taking advantage of cancellations, we have 

Pop, -Pi- (Pi - Rn)(Pi + Pi- Rii - R&rHPi - Rill 


which reduces to 

Pop, = Pi- W„ t (Pi - Rji). 


2.1.1 Implications of the Cross-Covariance R \2 

Under the assumption of common propagation models, the two local Kalman filters have three statistically 
independent .sources of information: measurements available to the first filter, measurements available to the 
second filter, and estimates propagated from the last measurement update [3]. While the propagated estimates 
are conditioned on different sets of measurements for the two filters, both use the same equations of motion 
and the same state noise spectral density. If the correlations in the two filters’ estimates arising from this 
common propagation are ignored, the fusion process will underestimate the covariance associated with the fused 
state estimate. Larger estimation errors may also result. These phenomena are illustrated in a simple example 
considered in the following section. 
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2,1.2 Equivalence to the Minimum Variance Estimator 


The optimal fused estimate derived above will be shown to be equivalent to a minimum variance estimate in 
which the estimate from one local filter is taken to be the prior mean and the other local estimate is taken to 
be a measurement. 

Given the stationary, ergodic, jointly Gaussian, random vector processes ,V(f) and >’(*), where >'(0 = 
[>‘i(0 > 3 (0] means x and y = [y\ jh] and covariance 


P = 



the minimum variance estimate of X(t) given a realization of y(t) is the conditional expectation E[X\y lt y 2 ], 

i.e., 


= E[X\ Vl) y 2 ] 

Xf(X \y x ,y,)dX. 


L 


According to Bayes’ theorem, the conditional density function in eq. 2.8 is given by 

„ yl , f{x,y) 

f(x\vi,y») = ■ 

where the joint and marginal density functions in eq. (2.9 are Gaussian, so that 


f(x,y) = 

/O') = 


, f x-X 1 T „ . f x-x 

I 9 '*[y-f\ p l>-y 


(2*) N / 2 VdZtP 

1 

dctPyy 


e -\(y-?) T p;,\y-y) 


(2.7) 

( 2 . 8 ) 

(2.9) 


( 2 . 10 ) 

( 2 . 11 ) 


and N and n are the dimensions of P and P rr , respectively. Carrying out the algebra in eq. 2.9 using eqs. 2.10 
and 2.11, and integrating eq. 2.8 yields 


xmv = « + e* y e y ~ l ( y - y). 


The covariance for this estimate cam be shown to be 




— Ptx ““ PxyPyy P 


Bar-Shalom has shown [6] that when X and y are both estimators of the same random process X } the 
cross-covariance term above may be found by taking iti as the prior mean for both X and y , i.e., x = xi and 
y = xi, and by taking X 2 as the realization of X i.«., y — xj, so that 


P,, = £[(x-xi)(xj -Xi) T ] 

= £[e 1 {(x-e,)-(x-e 1 )} r ] 

= £[ei(e t -ej) T ] 

= A - P\2' 


The autocovariance of y then becomes 

= £[(x 2 - xO(xj - Xi) T ] 

= £[(ei -e 2 )(ei -e 2 ) T ) 

= Pi + P'2 — Rl2 ~ R\2- 


Therefore, the minimum variance estimate for X is 

x mv = x, + (A - Rn)(Pi + P2- Rn - *£)- 1 (*2 " *1) 
= xi 4- Wofxii-2 — * 1 ) 

— Xopt , 
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and the covariance for this estimate is 

Prz\y ~ Pi " (Pi ~ P\2 )( A + P*2 “ #12 “ #[ 2 ) ' ( P 1 “ #12) T 

= A - ttV(p, - /? 12 ) r 

— Popt ■ 

Thus, the optimal fusion of two estimates is a minimum variance estimate in which the prior mean is taken to 
be xl and the measurement is taken to be xo 

Now. since xi and xo are each themselves minimum variance estimates, 

xi = E[X |*], 

*2 = E[X\ y 3 ], 

then 

x op , = E[«T|S»)], 

where the prior mean is given by E[X) = £'[A’|x l ]. Note that a global Kalman filter which processed both 
measurements directly would give the minimum variance estimate 

x KF = £[*!»!, ifa], 


which is identical to eq. 2.7. 

2.1.3 Equivalence to the Kalman Filter Estimate 

Is x op , = x/ff? In the Kalman filter, >(t) = HX(t)+V(t) by assumption, where £[V(t)] = 0 and £[V(t)V T (t)] = 
V6(t - r). Also, P ty = P„H T , P yy = HP tr H T + V, and f = Hx. (Here, x and P tx are assumed to have been 
propagated from the last measurement update by appropriate means.) Thus for the global Kalman filter, 

P::H(HP„H T + V)-\ 

x + K(y-Hx), and 

Pr: 

Prr - KHP„. 

If V is a diagonal matrix, another formulation for the Kalman filter exists in which the measurements are 
processed sequentially. Thus for Hi and V, corresponding to the ith component of y, 



K 1 = + Vir l , 

x ( ^ = X + K x ( yi -H.it), 

Ptx |yi = Ptx " E\H\Pzty 

and 

K 2 = P::\y>H 2 {H 2 P x r\ Vx Hl + V 2 )~\ 

* ( KF = *KF + *a(W ~ 

Ptx |ya = Ptx Ivi “ #2#2#rjr|yi- 

Thu s 

x ( ^ = (/ - K 2 H 2 )x { ^ F + K 2 (H 2 x + wa), 

which bears close resemblance to the optimal combination of estimates from two local Kalman filters, 


Xop, =(/- WV)*i + WV*2, (2 13) 

since Xi = x^ and xi was chosen to minimize E\V J V a ]. However, eqs. 2.12 and 2.13 will be strictly equivalent 
only when xi — x = 0; so, in general, x 0 pi ^ *KF- 


K i 

X-KF = 
Prr |y = 


REDUCTION OF DATA TRANSMISSION' REQUIREMENTS 


i } 


2.2 Reduction of Data Transmission Requirements 

In this section, some modifications which reduce the data transmission requirements of estimate fusion are 
described, along with some problems associated with using them. 

2.2.1 Use of Local Covariances to Update the Cross-Covariance 

In order to update the cross-covariance, the optimal derivation requires that the Kalman gain and measurement 
geometry matrices from the local Kalman filters be available to the fusion filter. A substitution can be made, 
however, which allows the local covariance matrices to be used for the cross-covariance update. 

In the previous section, it was shown that the cross- covariance is updated by 

R l2 = (I-K l H l )R l 2 (I-K 7 H 2 ) T . 

In many applications however, the Ki and H, may not be transmitted from the local Kalman filters to the fusion 
filter. Recognizing that 

Pi = (I-KiHi)Pi, 

an alternate form for the cross-covariance update is 

R 12 = A Pf‘ AaA“ ' A- 

This update formula may be employed for situations in which the local estimators provide estimates and covari- 
ances only. However, since two additional matrix inversions are required, speed and numerical accuracy may be 
compromised; careful consideration must be given to the issue of whether or not these disadvantages offset the 
decrease in data transmission requirements for a particular application. 


2.2.2 Optimal Scalar Gain Approximation 

In some cases, even the covariances from the distributed filters may not be readily available. An example is the 
typical Global Positioning System (GPS) receiver, which often provides as output only a state estimate and a 
figure of merit. This figure of merit is typically derived from the trace of the GPS filter’s covariance matrix, or 
some portion thereof. In this section, a suboptimal formulation of the date fusion equations will be given which 
is utilizes a figure of merit based on the covariance traces. 

Following the derivation of the optimal fusion gain, assume the following form for the fusion state update 

equation: 

x, u i = (1 - w)*! -I- WXj. 

Here w is restricted to be a scalar. Rewriting this in terms of the estimation errors, the covariance matrix for 
the fused estimate is 


P<u& — E[e iU > 

= E[{e x - we i 4- ti;e 2 }{et - we \ + u/e 2 } T ] 

= (1 — 2w 4- w 2 )P\ 4* w 2 P 2 + (tu — w J )(i?n + 


As before, the gain is determined by minimizing the cost function J = where 

trP $ = (1 - 2u; 4- u> 2 ) trP x 4* w 2 trP 2 + 2(u; - u> 2 ) trR i2 . 


Thus 


— = (-2 + 2 w) trP\ 4- 2u> trP 2 + (2 - Aw) trR l2) 
dw 


8J 


dw 


Wopi 


0 


trP x - trR x2 
trPi + trP 2 - 2 trR x2 


W 0 pt 


(2.14) 



16 


SECTIOS 2 . FUSIOS OF ESTIMATORS VSISG IDENTICAL MODELS 


An immediately apparent problem is the calculation of £rP 12 when P\ and P2 are not available. As shown 
in the following section, neglecting the cross-covanance matrix often does not introduce significant inaccuracies 
into the estimate. Since the use of a scalar gain will also introduce inaccuracies, simply neglecting the cross- 
covariance when using the scalar gain approximation may often be appropriate. An alternative may be to treat 
the trace of the cross-covanance as a tuning parameter whose size is chosen to force estimates calculated with 
the scalar gain to more accurately track estimates determined optimally. 

A more significant problem with the use of this approximation is that of scaling. Often the trace of a 
covariance matrix is dominated by only some of the covariance matrix’s diagonal terms (as is the case when 
errors in position and velocity are represented in the same covariance matrix). In these cases, the scalar gain 
may only be appropriately applied to those terms which dominated in its calculation. A solution to this problem 
is to calculate separate scalar gains for each related portion of the states and covariances. For example, an 
update for the position portion of the state could be calculated using the trace of the position elements in the 
covariance matrices, while a velocity gain could be calculated from the trace of the velocity elements. It is not, 
however, obvious how the portions of the covariance matrix which correspond to correlations between position 
and velocity should be updated. Once again, since the use of the scalar gain is a suboptimal procedure, it may 
be adequate to simply use the position gain for these correlations. This would be a conservative approach in 
comparison with not updating these correlations at all, zeroing them out, or using the velocity gain to update 
them. 


2.2.3 An Estimate Fusion Algorithm Which Resets Two Local Kalman filters 

In some applications, it may be possible or desirable to use a fusion filter to reset the local Kalman filters 
providing the state estimates to it. If the two local filters have the same state vectors, same propagation models, 
and are assumed to have the same, but uncorrelated, initial conditions, an efficient form of the fusion algorithm 
can be used. In this algorithm, there is no need to maintain a separate fusion filter, since the fusion update is 
applied to the local Kalman filters’ state estimates in the form of a reset. This algorithm is shown below. 

Algorithm 2.1 (Fusion Reset) Given xt = x 2 = x*, Pi = P 2 = P*, = 0 at t = t v 

The two local Kalman filters propagate the state and covariance matrix from t p to t* ustng the same model , then 
process individual measurements y 

xi(tfc) = x 2 (* t ) = x.(t k ) = m k ,t P )x.(t p ) 


Pi(tk) 


P,(t k ) = = $(<*,<p)A*((t,t P ) T + s(t k ) 

Ki = P.Hj{HiP.Hj + K)“ l 

Xi = x. + Ki(sfi - HiX.) 

Pi = ( I-K,Hi)P .. 


The cross-covanance ts propagated, and the estimate and covariance which optimally fuse this information are 
then calculated. The two Kalman filters are then reinitialized with this information: 


Ru(t k ) = S(f*) (Since Rn(t p ) = 0) 

Rn = (I - KiHi)Ri,(I - K 2 H 2 ) t 

w. = (A-AiaHA + A-fiia-fliir 1 

X. = (/-W.)*1 + W.x, 

P. = A - WAP, - Rf 2 ) 


Reset: xi = x 2 = x., Pi = P 2 = P* f R12 — 0- 



Section 3 


A Simple Example 


The objective of this section is to illustrate some of the principles and algorithms discussed in the preceding 
sections. 

An object which is falling in a constant gravity field is tracked to the ground by two radar systems colocated 
directly beneath the object. A schematic of the system is shown in figure 3.1. One radar station measures range 
only, the other range-rate only. Both stations process these measurements using (linear) Kalman filters. 



The two Kalman filters have imprecise knowledge of the gravitational acceleration; however, when they were 
designed, a state noise spectral density large enough to accommodate their imprecise knowledge of the equations 
of motion was given to both. 

The state equations of the true system are 

x = Fx + ii, 


where 


0 

0 0 I ’ 


u = 



the states are altitude and altitude rate, and g = 9.81m/sec 2 is the gravitational acceleration, 
this system used by the filters is 

x = Fx + u + Gw, 


The model of 


where 



■ 0 

1 ' 


r o 

— 

_ 0 

0 

, U - 

l -10 J 
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and w is a random vector with E{ wj = 0, iTJww 7 ] = L and GG r = Q. The state noise spectral density Q. 
was chosen such that the final covariance matrix which resulted from integrating the matrix Riccati equation. 
P = F P 4- PF t 4* Q, would bound the final errors between the true model and the filter model of the system 
For the following initial conditions, 



•501 


' 500 ' 

X 0 = 

. _0 ' 1 . 

x 0 = 

0 


an initial covariance matrix which bounds the estimation errors is 


Po = 


1 0 
0 0.01 


Now, the final states which result for a 10 sec trajectory are 



9.5 


0 

X J = 

-98.2 _ 

- */ = 

-100 


so that a covariance matrix which bounds the final estimation errors is 


Pf 


90.25 0 

0 3.61 


To satisfy the initial and final conditions on P, 


14.925 -1.800 ' 
-1.800 0.360 


which can be found by direct integration of the matrix Riccati equation. 

The discrete measurement models used by the filters are identical to the true measurement models. For range 
measurements, 

pi = H p Xi 4 - v Pt 

where v Px is a random variable with H p = [1 0] and E[v Pt vJ t ] = 20. For range-rate measurements, 

P* — E p X% + V p% 

where v p% is a random variable with H p = (0 1] and E[v Pt vJ .] = 2. 

The performance of the two filters for the given set of initial conditions is shown in figure 3.2. In this and 
subsequent figures, the solid line represents the estimation error and the dashed lines indicate the corresponding 
root mean square uncertainties from the error covariance matrix. 

Two methods of estimate fusion are investigated in this example: optimal estimate fusion, as described 
earlier in this section, and a suboptimal form of fusion in which the cross-covariance matrix is ignored. As 
mentioned previously, it is expected that ignoring the cross-covariance leads to underestimation of the state 
error covariance, and, therefore, possibly to larger estimation errors. These effects are demonstrated in figure 
3.3. When the cross-covariance is ignored, the dashed lines indicating the root mean square errors are discernibly 
smaller, indicating some underestimation of the position and velocity variances by the suboptimal fusion. Also, 
an increase in the velocity estimation error is clear in the later half of the trajectory for the suboptimal fusion 
filter. 

A geometric view of the mechanism of estimate fusion is shown in figure 3.4. Here, it can be seen that the 
covariance of the optimal fusion filter represents the intersection of the error ellipses of the covariances of the 
local Kalman filters. By comparison, the error ellipse for the fusion filter which ignores the cross-covariance 
terms is seen to represent a smaller area. Thus this filter postulates an uncertainty which is artificially smaller 
than could be expected from the information available to it. 

To better" quantify the effect of ignoring the cross-covariance matrix on underestimating the covariance of 
the fused estimate, a parametric study was performed. In this study, measurement variances one and two orders 
of magnitude above and below the nominal were employed in both the Kalman filters and the environment. As 
shown in figures 3.5 and 3.6, the effect of the cross- covariance matrix was found to decrease with increasing 
measurement accuracy. This phenomenon is explained by the fact that the Kalman filters weight their measure- 
ment updates more heavily relative to their propagations as measurement accuracy increases. If measurement 
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Figure 3.4. 1-Sigma error ellipses for the nominal case. 


variance were held constant, and process noise variance were increased, the same phenomenon would occur. In 
either case, measurements are very accurate compared to the propagated estimates, so the tracks being fused 
contain proportionally less of the propagation information which is common to both tracks. The tracks, there- 
fore, become less correlated, and their cross-covariance becomes reduced in comparison to their autocovariances. 
Measurement frequency is also a factor, as shown in figure 3.7. With frequent measurement updates, the Kalman 
filters are again forced to more heavily weight their measurements in comparison to their propagated estimates. 
Thus in situations in which process noise is very small compared to measurement noise, or in which measure- 
ments occur infrequently, the cross-covariance should not be ignored. Since in most filtering applications the 
opposite is true, the cross-covariance matrix may often be neglected. 

In the investigation just described, the cross-covariance update for the optimal fusion filter was performed 
using the inverse of the a pnort covariance matrices from the two local filters, i.e., 

Rn = PiPr'RuPf'P* 

This is the alternate update form described in the previous section. As mentioned in that section, use of this 
form may cause numerical difficulties due to the additional inverses required for this calculation. Therefore, as 
a validation exercise, the performance of a fusion filter which used the original cross-covariance update equation 

Ru = (I-K l H i)Rx 2 {1 - K 7 H 7 ) T 

was compared to the results shown above. This comparison is shown in figure 3.8. A closer inspection of the 
data showed exact agreement. This degree of agreement is due to the dimension of the covariance matrices in 
this example being merely two, since very accurate inverses can be calculated for 2 x 2 matrices. 

This example was also used to demonstrate the effectiveness of the scalar gain approximation. The actual 
cross-covariance was calculated, and its trace was used in computing the scalar gain. As shown in figure 3.9, the 
estimate generated using a scalar gain closely approximated the optimal gain in position. However, due to the 
scaling problem alluded to previously, very little of the “good” velocity information from the range-rate filter 
was allowed into the fused state estimate. 




v of Velocity Estimate 



Figure 3.6. Effect of range-rate measurement accuracy. 
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(Prior to Last Update) 



Figure 3 7. Effect of measurement frequency on error due to neglecting the cross- covariance. 



Figure 3.8. Comparison of cross-covariance update methods 













Section 4 


Application to a Lunar Rendezvous Problem 


This section presents a scenario in which the estimate fusion algorithm might be realistically applied: a ren- 
dezvous problem. In this problem, accurate relative state and inertial state information are required for good 
performance, yet both are not typically available from the same navigation processor. The sections below will 
describe such a problem and show how estimate fusion can be used to effectively improve the performance of a 
system which uses separate, stand-alone inertial and relative navigation processors. 


4.1 Description of the Problem 

An active spacecraft orbiting the Moon in a near-circular orbit is attempting to rendezvous with a passive 
spacecraft in a neighboring co-planar circular orbit. Initial uncertainties in the active vehicle s estimate of its 
own state corrupt its initial intercept maneuver, and it is desired to perform a midcourse correction once an 
updated state estimate is available from the vehicle’s navigation system. 

This navigation system is a distributed system consisting of two Kalman filters. One filter, referred to as 
the rendezvous filter, processes measurements derived from a radar system of range and elevation angle to the 
passive vehicle, whose state is assumed to be perfectly known. The other filter, referred to as the ground beacon 
filter, processes measurements of range from two beacons on the lunar surface whose positions lie on the vehicles 
ground track and have been previously surveyed to high precision. These measurements are derived from the 
transit time of a signal broadcast by the beacon. Both filters use the same simple spherical gravity model, which 
is augmented by Gaussian process noise. A simple model is also used for the environment’s dynamics, which 
consists of a simple spherical gravity model augmented by Gaussian process noise and a bias term. These models 
will be described more fully in section 4.3. 

It is expected that the rendezvous filter will produce accurate estimates of the relative position and velocity 
between the two vehicles but inferior estimates of the inertial states of both vehicles. The occurrence of large 
inertial state errors could lead to inaccurate maneuver targeting solutions as well as to a large buildup of relative 
state errors during propagation intervals [10]. To prevent the occurrence of such deleterious effects, it is desired 
to rectify the rendezvous filter through the use of a fusion algorithm which optimally combines the estimates of 
the rendezvous and ground beacon filters. The optimal state estimate and error covariance will then be used to 
reset both filters. 

The passive vehicle’s orbit has a radius of 2 lunar radii. The active vehicle begins its maneuver 100 km behind 
and 50 km below the passive vehicle, as measured in a curvilinear target-fixed coordinate frame. The transfer 
is constrained to occur over a 30 degree arc, beginning at longitude 345 degrees and ending at longitude 15 
degrees. The ground beacons are located at longitudes 330 degrees and 30 degrees, remaining visible during the 
entire maneuver. The selenographic frame to which the ground stations are fixed is assumed to be nonrotating. 
an approximation due to the short length of the maneuver. The orbit transfer takes approximately 25 minutes. 
The nominal maneuver is shown in figures 4.1 and 4.2, which depict the maneuver from inertial and target-fixed 
viewpoints, respectively. 


4.2 Rendezvous Targeting 

Hill’s equations are used to calculate the initial and midcourse intercept maneuvers. In this section, these 
equations are given, and a curvilinear target-centered coordinate system is defined. 
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Figure 4.2. Rendezvous maneuver, target-fixed viewpoint 








4 2. RESDEZVOCS TARGETISG 


4.2.1 Target-Relative Curvilinear Coordinates 

Let the orthogonal target-relative radial and downtrack coordinates of the chaser be represented by u and C . 
respectively. In the figure 4.3 below, an alternate system of coordinates is depicted which represents the rhaser 
position in a frame that follows the curvature of the target vehicle orbit track. In this system, 

u = the radial distance of the chaser above the target vehicle orbit track, and 
v = the distance of the chaser from the target along the target vehicle orbit track. 

From the figure, it can be seen that 

v - p t 6 , 

where p t = ||r t ||. It follows that, since p t is a constant, 

v = p t 0. 


Next note that 


Solving for 0 and differentiating yields 


tan 0 = r • 

Pt + u 


‘{p t 4- u)f> “ t^u 


(P« + u) 2 


cos 2 0. 



From the definition p e = ||r c ||i w c have 

Pc = Pt + u, 


and that 


p c cos B = p t + u. 


Thus 

and 


(p t -h u) cos 9 = pt 4- u, 
Pt + u 


cos 0 


~ Pt- 
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Differentiating the above expression for u yields 


u + v9 


Expressing the target-relative position and velocity of the chaser spacecraft in terms of the curvilinear coordinates 
just defined is a convenient way of specifying rendezvous maneuvers. However, to integrate the equations of 
motion, the relative state defined in this manner must be expressed in inertial coordinates. 

4.2.2 Conversion From Target-Relative Curvilinear Coordinate System to 
Planetocentric Inertial Coordinate System 

A procedure for converting position and velocity vectors expressed in target-relative curvilinear coordinates into 
an inertial system is given below. 

Algorithm 4.1 (Position Conversion) Given u, £>, r t , find r c . 

L * = 

* lNI = IMI + s 

3. u = ||r c 1 1 cos 8 - ||r,|| 

4- v = (||r t || + u) tan 8 


2 j, _ cos / - sin / 

~~ sin / cos / 


6. r c , = T 


7. r c = r, + r ct 

Algorithm 4.2 (Velocity Conversion) Given u, v,r t , find r e . 


2. u = u cos 8 — 8v 


3- Ucon = ||r,|| + u 


4 ■ v = + p- 

cos " 1 8 “««*» 


5 x re ‘ = Vlwi 5 1 "C 


6. r c t = T \ _ + w uC X Tct 


7. r c = r t + r ct 


Using algorithms 4.1 and 4.2, we can express the target- relative curvilinear positions and velocities first in terms 
of the target-relative inertial states, r ct and r c( , and then in terms of the planetocentric inertial states, r c and 
r c . The latter will be used for numerical integration of the equations of motion of the spacecraft. 
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4.2.3 Targeting Method 

The relative motion of two bodies in neighboring near-circular orbits can be described using Hill’s equations 
[LI], the in-plane components of which are 

u — 2nt> — 3n 2 u = /u, and 

v + 2nu = ft ■ 


The notation is the same used previously with n = y^/||r t || 3 The force-free integrals of these equations can 
be solved for the initial relative velocity which results in an intercept of the two trajectories at some later time. 
This solution, which can be found in Kaplan [11], is as follows: 

i [6u 0 (nt - sin nt) - v 0 ]nsmnt - 2nu 0 (4 - 3cosnt)(l - cos nt) 

V ° ~ (4sin nt — 3nt) sin nt + 4(1 — cos nt) 2 

nu 0 (4 - 3 cos nt) 4-2(1- cos nt)v 0 

U 0 = 7 • 

sin nt 

To complete the rendezvous targeting, the required relative velocity for intercept calculated using Hills equations 
is converted to an inertial velocity using the method described in section 4.2.2. 

4.3 Models 

The measurement and dynamics models used by the environment and the navigation filters are described in 
detail in this section. 

4.3.1 Environment 

The environment portion of the simulation consists of the computation of true dynamical quantities and the 
use of these quantities to simulate measurements. The dynamics consist of a simple spherical gravity model, 
augmented by Gaussian process noise and a bias term. The measurements are of range from two beacons on the 
lunar surface, presumably derived from the transit time of signals broadcast by the beacons. 

Dynamics 

A simplified model of the lunar gravity is used in the environment. The model consists of a spherical gravity 
field corrupted by Gaussian noise and a bias in the gravitational parameter. The chaser and target vehicle 
accelerations are given by 


MO = + M) r ‘ ( * |,3 ~ *%..»> (°» *)■ 

IMOII 

r«(t) = ~(/i + (°.l)- 

IMOII 


and 


The variable tj (0, 1) represents a vector of uncorrelated random variables, each element of which is characterized 

by a normal density function. Also, 6fi = 10“V ^ = 0.001/i/||r c (t)|| • 

These two second-order nonlinear vector differential equations can be written as a collection of four first-order 
vector equations by defining an environment state vector as follows: 





r e (t) 

Tc(t) 

MO 

MO 


Then x eno (f) = f(x em ,(0) + w env (t), where 
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Table 4.1. Integrator Comparisons, Noise Off 


Integrator a 

Execution 
Time , sec 

.Vo. of 
Steps 

Tolerance b 

Step Size , c 
sec 

ode23 

11.88 

l 0 

10~ 1J 

- 

ode45 

1.98 

7 

10" 1 

- 

ode78 

30.02 

52 

10- 1 

- 

nlz33 

2.23 

20 

- 

0.5 


3 10 second integration of circular orbit. 

6 Max. tolerance setting required for accuracy comparable to best integrator. Applies only to 
variable-step integrators. 

c Step size setting required for accuracy comparable to best integrator. Applies only to fixed- 
step integrator. 


Table 4.2. Integrator Comparisons, Noise On 


Integrator a 

Execution 
Time, sec 

No. of 
Steps 

Tolerance 

Step Size, 
sec 

Norm of 
Errors b , cm 

ode45 

2.17 

8 

Hr* 

- 

2.71 

ode78 

3.79 

6.8 

10" 6 

- 

0.886 

nlz33 

2.20 

20 

- 

0.5 

0.872 


a Mean of 10 cases. Adequate results could not be achieved using odetS . Noise was applied radially, 
with standard deviation of 1% of the gravitational acceleration. 
b Error taken as difference from mean (i.e. t noise off) trajectory. 




J 

0 

f(x enu (<)) = 

(p + ^A*) ||r*(f )]| J 
r ,(t) 

, w <nv (l) = 

(0,1) 

0 




. (0, 1) . 


with t p <t < tk and an initial condition given by x #flt ,(t p ). Note that thia motion is restricted to a plane, so by 
choosing an inertial basis for these vectors which is aligned with the plane of motion, only two scalar components 
are required for each vector. Thus x« n v contains eight states. 

Numerical Integration 

Because of the noise terms in the dynamical equations, a numerical technique was chosen to integrate the 
environment state vector. Three variable step methods and one fixed step method were evaluated for accuracy 
and speed in performing this numerical integration. The variable step methods use pairs of Runge-Kutta formulas 
along with Fehlberg’s coefficients [12]. These three methods use 2nd- and 3rd-order, 4th- and 5th-order, and 
7th- and 8th-order pairs of formulas, and are designated odeSS, odc45, and ode 78, respectively. The fixed step 
method, designated nlzSS , uses a formula derived by Nystrom with coefficients determined by Lear [13]. As 
tables 4.1 and 4.2 show, nlzSS was found to have superior accuracy and similar speed in comparison with the 
other integrators, so it was chosen for this application. 

Simulated Measurements 

Direct measurements of noise-corrupted range were assumed to be available. To facilitate the calculation of 
these measurements, a range function was defined in terms of two position vectors, r and s, 


G„(r, s) = s) T (r - s). 
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Then the range to ground beacon i from the chaser vehicle is 

PbJtj ) = G p (r bi . r Un ,(<;))+ <?p t n(0. 1) 
The range to the target vehicle from the chaser is 


Pt(tj) = Gp(r 1 ...(t J ),r c ,„.(< J )) + <T„r?(0. 1). 

The angular measurement for the rendezvous filter is assumed to be made with respect to a perfectly aligned 
inertial platform. Therefore, if p = - r the noise-corrupted angular measurement can be 

expressed using the inertial components {px<Py} of p as follows: 

9 t (t j ) = arctan — + <7»,r7(0, 1). 


4.3.2 Extended Kalman Filters 

The two Kalman filters in this simulation use dynamics and measurement models which approximate the envi- 
ronment models. However, the filters do not have knowledge of, nor attempt to estimate, the gravity bias term. 
The filters must also propagate and update the covariance matrices associated with their state estimates. 

In subsequent descriptions of the filter models, the following notation will be used: 

x(t k ) = x(t*)|(y(<*-i).y(*k- 2 ). ■ ) 

x(f*) = x(t k )l(y(tk),y(tk-i),...) 

That is x(t k ) represents the estimate of x(t k ) conditioned only on past measurements, or the a prior, estimate, 
whereas x(t k ) is the estimate conditioned upon the current measurement as well, or the a posteriori estimate. 
This same notation will be applied to all quantities estimated by each filter. 


Dynamics 

Since the filters’ best a pnon estimates for the zero-mean noise terms in the dynamics are zero and since the 
filters are denied the knowledge that a gravity bias exists, their model of the chaser vehicle acceleration is 
Keplerian, 

• u\ _ A"e(«) 

IMOlf 

As with the environment model, this second-order vector equation can be reduced to a pair of first-order vector 
equations which define the derivatives of position and velocity. The state vector for the ground beacon filter, 
therefore, is 


x m * I r ‘.(*) 

X,(<) " L r« f (0 

which has four scalar states. Again (t) = f(xj(t)) where 

r *«.(*> 


!*■«.(*) 


with t,, < t <t k and an initial condition given by Xj(t p ). 

The rendezvous filter requires knowledge of the target vehicle position in order to calculate its range mea- 
surement. This quantity is calculated using the same Keplerian acceleration model as is used for the chaser 
vehicle. Thus the rendezvous filter state vector is 


Xr(t) = 


fe,(0 

TcAt) 
r t r (t) 
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n 


which has eight scalar states As before x r (t) - f(x r (0) where 


f(x r (<)) 


Tc r (t) 

_ fc.fO 

^iirTTmjp’ 

*«,(0 

-u-EjAEL. 

H|r, r (<)|| J J 


with t p < t < tk and an initial condition given by x r (t p ). 

Although not required due to the Keplenan acceleration model used by these filters, states corresponding to 
chaser and target velocities were numerically integrated using nlz33. This choice was made to ensure compati- 
bility with the environment model, provide flexibility for incorporation of other gravity models, and simplify the 
simulation design. 


Covariance Propagation 

Both the rendezvous and ground beacon filters maintain a covariance matrix associated with the states they 
are estimating, i.e., the chaser vehicle position and velocity. Thus each filter must propagate a 4 x 4 covariance 
matrix between measurements. These covariance matrices obey the generalized matrix Riccatti equation [14] 

P(t) = F(t)P(t) + P(t)F T (t) + Q(t). 

An approximation to the solution of this differential equation is 

P(t k ) = i(t k ,t p )P(t p )* T (t k ,t p ) + S(t„). 

The state transition matrix, *(U,t p ), is approximated by assuming the dynamics are constant over the time 
interval A t = t k -t p . As a result, a well-known power series [15] can be employed where terms of order At and 
higher have been truncated: a 

*(tk,tp) = J + F(x(t p ))bt + F J (x(t p ))— . 


In this series, 


F(x(t P )) = 


— I 

[ OjxJ 
G(x(t p )) 


Ilx2 1 

OjxZi J ’ 


where 


G(x(f,)) = -- 






■yr c (< p )rf (fp) 


According to Lear [19], this approximate solution is accurate to 10% for At - 10 sec and 
for a 1 rev propagation in a near-Earth orbit with a spherical gravity field. 

The state noise covariance matrix cam be expressed in terms of the state noise spectral 

the integral relation 

S(t k )= [ <J(t,r)Q(r)4 T (t,r)dr. 


to 1% for At = 1 sec 
density matrix using 


An approximation for this integral can be found by using a first-order truncation of the transition matrix and 
by assuming the integrand is constant over the interval of integration, 

S(t k ) = [{/ + F{x(t p ))At) Q(t p ) {/ + F(x(t p ))At} T ]At. 


For the filters in this simulation, process noise is added only to the acceleration states. Thus 


Q(tk) 


02x2 02x2 

02x2 Q r c 
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where 


Qr c - 


°r 

0 


0 

o 

a: 


Thus for this state noise spectral density matrix, the approximation given above for the state noise covariance 
matrix becomes 

A t 3 Q Fc A * 2 


S(t k ) = 


Qr e h 

Q r c-\t 2 Q rc Ai J 


Measurement Processing 

The filters 1 models for estimated measurements resemble the environment models, with the exception that the 
filters 1 best estimates for the zero-mean noise terms are zero. Thus for the ground beacon filter, the estimated 
range to ground beacon i is r Cff (*,)) so that this filter’s measurement residual is 

G p{ r&, , r Cg (tj )) i 

and the range measurement residual for the rendezvous filter is 


&pt{tj) — Pt{t ; ) - Gp(rt r (^), f Cr (t ; ))• 

Similarly, for p = G fi (r tr (tj) y r Cr (tj)) the angular measurement residual for the rendezvous filter is 

SOAtA = - arctan 

Px 

In order to process this measurement, each filter must calculate the matrix of partial derivatives of its 
measurements with respect to its states. For convenience, a unit range vector function is defined as 

, , a (r - s) T 

8 ' (r ' s)= c7rT)' 

The measurement partial derivative matrix for the ground beacon filter using g p (r, a) is 

A dc 


For the rendezvous filter, we can define 




/ \ * 

g#(r->) = 


r - S' 


-g/*(r»,. Tc,(tj)) 0 0 
-gp(n„ f Cf (t>)) 0 0 


-y[(rY - *y) ~ ( r x - **)]• 


Then the measurement partials matrix for the rendezvous filter can be written as 

ns,) £ 


&r(tj) 


-g ,(*«,(*,• ).fc.(«i)) 0 0 
g«(r« r (<>),r e ,(t,)) 0 0 J ’ 


where y r = [pt,9t] T - 

The matrices calculated above are used in conjunction with the a prion covariance matrices in the calculation 
of the Kalman gains for each filter. For the ground beacon filter, the Kalman gain is given by 

= P } (t,)H } (t ] ) T (H ) (t i )P,(t ] )H,(t 1 ) T + V t )~\ 


and for the rendezvous filter 


Kritj) = Pr(t j )Hr(t i ) T {Hr(t j )Pr(t j )H r (t j ) T + V r )~ l . 

Here, V g and V r are the measurement covariances for the ground beacon and rendezvous filters, respectively 
Note that since the measurements are assumed to be uncorrelated, these matrices are diagonal. 
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Extended Kalman Filter Updates 

The standard Kalman filter is designed to operate on linear systems and as such cannot be applied to the esti- 
mation problem considered here, which has nonlinearities in both its dynamics and its measurements. However, 
an approximate method known as the extended Kalman filter (EKF). which has a similar structure to the linear 
Kalman filter, can be applied here. For the ground beacon filter, the EKF state update is given by 


X 3 {tj) = Xg(tj) + Kg(tj)6p b (tj). 


Similarly, for the rendezvous filter 

= *r(tj ) + 

The error covariance matrices associated with the two filters are then updated using 


Pi(h) = (I - and 

M*i) = ))£•(*,). 

A detailed description of the extended Kalman filter can be found in many standard texts, e g., [17], [18], [19]. 

4.3.3 Kalman Filter Fusion and Reset 

A form of the estimate fusion reset algorithm developed in section 3 is used to fuse estimates containing inertial 
information from the ground beacon filter with the relative state information present in the rendezvous filter. 
As with the simple tracking example of section 3, a suboptimai form of the fusion algorithm which ignores the 
cross-covariance matrix is compared to the optimal fusion method. 

First, the optimal fusion gain matrix is calculated as 


W opt (tj) = (Pritj) - ))(*(<;) + P,(tj) - Ropt(tj) - Ratify 1 , 


where 

Ropt(tj) = (/ — Kr(tj )H r (tj)) Ropt(tj) ( I — K 9 (tj)H 9 (tj)) T . 

The cross-covariances are propagated from the last reset using 


Ropt(tj) — ytp)Ropt(tp)$ T (tj > *p) + S(tj ), 


but at the end of each fusion update, the cross-covariance is reset to 0, i.e. Ropt(tp) = 0, so that 


Ropt(tj ) — S{tj ). 


Since the suboptimai fusion filter ignores the presence of the cross-covariance, the suboptimai gain is simply 

W. ub (t j ) = Pr(t j )(Pr(t j ) + P t (t j ))- 1 

The fusion state update for both the optimal and suboptimai algorithms is performed using 

= (/ - W.it^Xriti) + W.(*i )*,(*,). 

where the subscript is used since the optimal and suboptimai state updates have the same form and no dis- 
tinction is necessary. Note that, as with the EKF, the update appears to be made directly on the nonlinear states 
despite the assumption of linearity made in the derivation of this algorithm. However, within the assumptions 
made in the EKF approximation, the fusion update equation is valid. 

The covariance matrix associated with the fused state estimate is calculated next using both the optimal and 
the suboptimai method: 


Ropt{tj ) — {I “ )) Rr{tj ) + W 0 pt(tj )Ropt(tj )> and 


Psub(t j ) = (I-W sub (t } ))P r (t } ). 
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Table 4.3 Filter Design Parameters 



rendezvous 

filter 

ground beacon 
filter 

Initial RMS Pos. Error, m 

100 

100 

Initial RMS Vel. Errors, m/sec a 

2 

2 

RMS Accel. Noise, m/sec 2 b 

0.1 

o.l 

RMS Range Meas. Error, m 

30 

30 

RMS Ang. Meas. Error, rad 

0.15 

- 

Propagation Interval, sec 

10 

10 

Measurement Interval, sec 

60 

60 


a Initial errors are applied equally in all channels 
b Noise assumed to be equal in all channels 


Finally, the filters are reset by replacing their estimates of the chaser vehicle states with the estimate derived 
from fusing the two estimates and by replacing their error covariance matrices with the covariance resulting from 
estimate fusion. Thus for the optimal fusion update 


itfjtj) = x opl (tj) : (state update) 


p?\h) = K 

and for the suboptimal fusion update, 

= x.u *(*>), 


(covariance update) 


(state update) 


P r '“‘(tj) = P.ui(tj), (covariance update) 

p; u %) = Au»(*>). 


4.4 Results 

The problem just described was studied by coding a simulation of it in Matlab ; this simulation was executed 
on a DECStation 5000 workstation in the Spacecraft Navigation and Rendezvous Laboratory at the University 
of Texas at Austin. The ground beacon and rendezvous filters were manually tuned to achieve satisfactory 
performance. The deaign parameters chosen through this process are shown in table 4.3. In the following, the 
performance of these filters operating in a stand-alone mode, unaided by estimate fusion, is compared to their 
performance when reset using optimal and suboptimal forms of the estimate fusion algorithm. 

In figure 4.4, typical performance of the rendezvous filter and the ground beacon filter without the use of 
estimate fusion is shown. The solid lines represent estimation errors and the dashed lines the corresponding 
root mean square uncertainties of these errors as derived from the error covariance matrices. Shown here are 
radial and downtrack components of the errors in the estimates of chaser vehicle inertial position. This figure 
may be compared with figure 4.5, which shows the estimation errors for the same quantities as calculated by 
the fusion-aided rendezvous filter. This filter is reset using both optimal and suboptimal forms of the estimate 
fusion algorithm (recall that the suboptimal form ignores the presence of correlations between the two Kalman 
filters). Since both filters use the same models and start with the same initial conditions, the ground beacon 
filter which has been reset will exactly match the rendezvous filter so it is not shown. As indicated by the root 
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mean square bounds on the estimation errors, aiding the Kalman filters using either of the fusion reset algorithms 
has brought significant performance improvements, most notably in the radial channel of the rendezvous filter 
during the first half of the maneuver. However, note that fairly large estimation errors are present after the last 
measurement in the downtrack channels of the fusion-aided filters. Note also that no noticeable degradation in 
accuracy occurs if correlations are ignored in the fusion algorithm. 



Figure 4.4. Inertial position estimation errors for unaided Kalman filters. 

The velocity estimation, shown in figures 4.6 and 4.7, was quite good even for the unaided filters. However, 
the same trends noted above may be discerned through careful observation. 

Relative state estimation errors, expressed in terms of coordinates in the direction parallel to the line-of- 
sight between the two vehicles and its normal direction, are next shown in figure 4.8. The unaided rendezvous 
filter is compared to the rendezvous filter reset by the optimal fusion method. Since the target vehicle states 
are perfectly known to these rendezvous filters, trends similar to those noted for the inertial state estimates 
are observable here. In particular, it is clear that the unaided filter is nearly equivalent to the fusion-aided 
filter in accuracy along the line-of-sight to the passive vehicle, but its accuracy normal to the line-of-sight is 
heavily range-dependent. Resetting this filter using an estimate which contains the data from the ground beacon 
filter greatly enhances its ability to accurately estimate the relative state components which are normal to the 
line-of-sight. 

A great deal more insight into the process of estimate fusion may be gained through visualization of the 
optimal combination. To this end, projections onto the position plane of the hyperellipsoids corresponding to 
the error covariance matrices are shown in figures 4.9 - 4.12. In section 3, similar plots show that the optimal 
fusion algorithm determines the error ellipse which best fits the intersection of the error ellipses from the two 
filters being fused. Now it would appear that a best fit hyperellipse is being achieved, which is not necessarily 
the best fit ellipse when projected onto a plane. Note that the rotation of the ellipse deriving from the unaided 
ground beacon filter, indicated by the dashed line, corresponds to the orbital motion of the chaser vehicle. 
The ellipse corresponding to the unaided rendezvous filter maintains a fairly constant orientation, since the 
line-of-sight between the two vehicles is fairly constant, but as the relative distance closes, the accuracy of the 
angular measurements in this filter exceeds the accuracy of its range measurements such that its semimajor and 
semiminor axes become transposed. Note also the close match in size and orientation of the ellipses corresponding 
to optimal and suboptimal fusion updates. 
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Figure 4.11. 1-Sigma error ellipses midway through maneuver. 



Figure 4.12. Final 1-sigma error ellipses 









Section 5 


Conclusions 


5.1 Summary 

This document presented a new derivation of an algorithm which fuses the output of two Kalman filters based 
on optimization theory. Through the use of a simple example of tracking a falling body, this method has been 
shown to provide an estimate whose covariance is the best fit covariance to the intersection of the covariances 
from each filter. Several modifications to the algorithm which reduce data transmission requirements were also 
presented, including a scalar gain formulation and a form of the algorithm which resets the two Kalman filters. 
The latter of these was then applied to the problem of lunar rendezvous, in which one Kalman filter processes 
relative measurements and the other inertial measurements. Although several restrictive assumptions were 
made in the analysis, including perfect knowledge of the target vehicle’s state, promising results were obtained, 
demonstrating the effectiveness of the fusion reset algorithm. 


5.2 Areas for Future Research 

Two limiting assumptions made in this analysis were (1) no unknown parameters must be estimated to process 
the measurements and (2) the state of the target vehicle is known perfectly. Although neither is likely to be true 
for a realistic application, these assumptions were required due to the limitation of applying the fusion algorithm, 
as presented here, to combining filters which have the same state vector. Below, some of the consequences which 
can result from making these assumptions are addressed. 

As an example of problems induced by unknown parameters, consider the following case: a clock bias in 
the on board receiver for the signals from the ground beacons causes an inaccurate determination of the signal 
transit time. Neglecting any frequency drift in the on board clock, this bias can be modeled as a constant, but 
unknown, range bias which is the same for both ground beacons. Although the rendezvous radar is a two-way 
ranging system, clock drift could also cause a range bias to occur for this measurement type. If these biases 
are ignored in the navigation filters, results like those shown in figures 5.1 through 5.3 may be obtained. Here, 
range biases of approximately 35 meters and 19 meters are present in the measurements of the ground beacon 
and rendezvous filters, respectively, but these biases are ignored by the filters. As the plots show, these biases 
appear to directly corrupt the position estimates of both aided and unaided filters. 

Although these biases could be estimated by the Kalman filters, it is not clear how to utilize this information 
in the fusion reset algorithm presented herein. If only those states which are common to both filters are reset, 
these states will be incompatible with the states unique to each filter since their correlations will become invalid 
Alternatively, a centralized fusion filter could be utilized to estimate all the states present in both filters. The 
difficulty here is that the algorithm as presented treats one of the filters as the a pnon data source and the 
other as a measurement source (section 2.1.2). It can be shown that the rank of the a posterior t covariance in 
such an estimation scheme is equal to the rank of the a pnon covariance. Since the filter taken to be the a 
prion data source does not contain all of the states modeled by the fusion filter, the covariance resulting from 
the fusion process will have insufficient rank. A potential solution to this deficiency would be to reformulate 
the fusion algorithm such that an a pnon covariance for all the states were maintained. Such an approach has 
been taken by several authors, including Speyer [2], Willsky [3], and Wei and Schwarz [20], However, such an 
approach requires the additional processing of a centralized fusion filter which is not needed for the fusion reset 

method. 
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Figure 5.1. Effects of unmodeled measurement biases on relative position estimation errors. 
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Figure 5.3. Effects of unmodeled measurement biases on inertial position estimation errors for fusion-aided 
Kalman filters. 

An example of the effects of errors in the target state is next shown in figures 5.4 through 5.6. As predicted 
in [10], the rendezvous filter is able to accurately estimate the relative states but not the chaser’s inertial states. 
Unfortunately, when the accurate inertial information from the ground beacon filter is fused with the accurate 
relative state information, degraded estimates of both result. This occurs because the target states are assumed 
to be perfect in the rendezvous filter, so that the update resulting from the fusion algorithm is artificially 
constrained to be applied entirely to the chaser vehicle inertial states. If both the target and chaser states could 
be updated, then it seems likely that better results would be achieved. This would require the addition of target 
state estimation to the rendezvous filter, making its state vector incompatible for fusion with the ground beacon 
filter by the present method. 
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Figure 5.5. Effects of target state errors on inertial position estimation errors for unaided Kalman filters 
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A.l Abstract 

A new derivation of an algorithm which fuses the outputs of two Kalman filters is presented and placed within 
the context of previous research in this field. Unlike the work of other authors, this derivation clearly shows the 
combination of estimates to be optimal, in the sense of minimizing the trace of the fused covariance matrix. The 
algorithm assumes that the filters use identical models and are stable and operating optimally with respect to 
their own local measurements. The method is also demonstrated to provide an estimate whose error ellipses are 
contained within the intersections of the error ellipses from each filter. Modifications which reduce the algorithm’s 
data transmission requirements are also presented, including a scalar gain approximation, a cross-covariance 
update formula which employs only the two contributing filters’ autocovariances, and a form of the algorithm 
which resets the two Kalman filters. The latter of these leads to a modification of the Kalman filter, which 
we call the reinitialized Kalman filter (RKF). This filter is then applied to the problem of lunar rendezvous in 
which one stand-alone Kalman filter processes relative measurements and another inertial measurements. These 
results demonstrate the effectiveness of the optimal RKF and am RKF which ignores the presence of correlations 
between the two contributing filters as well as that of the scalar gain approximation. 


A. 2 Introduction 

An area of increasing interest in systems research is that of combining data from a distributed network of local 
sensors and/or estimators into a global estimate which combines the information available to each system in a 
complementary fashion. Such techniques have a wide range of applications, including distributed process control, 
fire control, remote sensing, and managing data from redundant systems. Another interest in data fusion research 
is motivated by the proliferation of black-box navigation systems such as most Global Positioning System (GPS) 
receivers. A desire of contemporary spacecraft designers is to combine such off-the-shelf systems in a distributed 
architecture in such a way that the measurement availability and geometry of the various systems complement 
one another in some optimal fashion. Typically, modification of the outputs of these systems to meet data 
fusion requirements is not a cost effective option, so the desire exists to combine the available information into 
a globally optimal estimate which may then be used to reset the local processors. In this way, modifications are 
made outside the existing system, rather than inside. Such a scenario is the focus of the work presented here. 

Several approaches to the problem of data fusion are possible, and many have been considered in the literature. 
The most basic form of data fusion occurs in the globally optimal Kalman filter, which optimally combines raw 
measurement data from various sources. A basic assumption, however, is that the measurements are uncorreiated 
in time. If, rather than raw measurements, it is desired to optimally combine the estimates from several Kalman 
filters, the zero autocorrelation assumption will be violated. This problem has been referred to as filter cascading 
[21]. Various solutions have been presented to this problem, including the works of Willner, et al. [1], Speyer [2], 
Willsky, et al. [3], Bar-Shalom, et al. [5], [6], [7], and Alouani and Bird well [4]. Other possibilities exist, such as 
the case in which some data sources are raw measurements and others are the outputs of estimation schemes. 
This class of data fusion has been labeled hybrid fusion and has been examined both by Willsky, et al. [3], and 
Blackman [8]. 

In this work, an optimization- based approach is taken to solve the problem of combining the estimates from 
two Kalman filters which are tracking the same target but possibly have different measurement sets. Several 
modifications to the algorithm derived in this way are presented which reduce data transmission requirements 
Finally, the algorithm is applied to a navigation system which optimally combines the state estimates of two 
Kalman filters on board a lunar orbiting spacecraft. The spacecraft state estimates obtained with this method 
are used to facilitate a rendezvous mission with another lunar orbiter. 

A. 3 Problem Statement 

Two continudus/discrete extended Kalman filters [17], [19] are operating on the system described by 

x(«) = f(x(t)) + w(t); w(t) ~ 7V(0, W(t)6(t - r)), 

Yij =h l7 (x(t, )) + v lj ; v 1; ~ jV (0, 

y 2j =h 2y (x(tj)) + v 2; ; v 2; ~jV(0,1 Z 2j 6 jk ), 


(A.l) 
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VJ 


where filter l processes the discrete measurements y l; . filter 2 processes the discrete measurements y 2j . and 

j = 1,2 Also, the notation r — 4 V(0, R) implies that r is a zero-mean, white gaussian vector random process 

with R as its covariance, 6(t - r) is the Dirac delta, and 6 jk is the Kronecker delta. Hereafter, the subscript 
j' will be omitted to simplify the notation. Each filter 0 = 1.2) updates its state estimate and state error 
covariance matrix at time tj using 

x* = x t + A',[y, - h,(x, )] (A. 2) 


and 

Pi = [I-KiHi(Xi)]Pi t 


where /v, is the Kalman gain. 

K, = P i Hj'(x,)[H,(x,)P > Hl(x l ) + V,]~\ 


and 


Hi(Xi) 


dhiWtj)) 

dx(tj) 


*,(<;) 


Here, and subsequently, the notation ‘ ‘ ’ represents an estimate immediately posterior to a measurement update, 
and indicates an estimate prior to the incorporation of a new measurement. The filters propagate their states 
and covariances between measurements via 


i,(t) = f(xi(0), 


with x,(<j) as an initial condition and 


A(0 = *(Mj)A(*i)* T (Mj) + S(0, 


(A.3) 


where 


*(*,*;) = F(*i{ 0)*(M;); *(*;,*;) = I, 




dx(t) 


*.(«) 


and t 

S(t)= f *(t,r)W(r)* T (t,T)dT. 

It is assumed that the filters are stable and operating optimally with respect to their own measurements and 
use the same system model with the same initial states and covariances. The problem at hand is to fuse the 
outputs of the two Kalman filters, as depicted in figure A.l, in an optimal fashion. 


A. 4 Problem Solution 

A.4.1 Optimal Combination of A Posteriori Estimates 

In order to combine the estimates from these two filters in some optimal fashion, we assume a form for the 
optimal combination of the filters’ a posteriori estimates as follows: 

x. = (/- W)xi + Wx 3 , (A. 4) 

where the fusion gain matrix W is to be determined and the subscript indicates the optimal combination 
Next, we define the a posterior* estimation error as 

ei = x - x t ; i = 1, 2, *. (A. 5) 


From eq. A. 4 and eq. A. 5, it follows that 


e„ = (I-W)ex + We 2 . 


(A. 6) 
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Figure A.l. Estimate fusion schematic. 

The state error covariance matrix for the optimal combination is defined to be 

P. = E[e. ef], (A. 7) 

Computing P, in eq. A. 7 using e. in eq. A. 6 yields 

P. = Pi- (Pi - R)W t - W(Pi - R T ) (A.8) 

+ W(Pi + P 7 -R-R T )W T , 

where R = E[e , ejj*], which is the a posteriori cross-covariance matrix. 

To determine P, recall eqs. A.l and A. 2, 

Xi = Xi + tfi[hi(*) + v < -hi(x<)] 

= x< + /fi[h,(x) -h<(xi)] + /fjV, 

« Xj + KiHiki + Ki\i\ i = 1, 2, (A. 9) 

where e* = x - x» and the dependency of Hi on Xi has been suppressed for clarity. Rewriting eq. A. 9 in terms 
of estimation errors gives 

«• = e* - KiH& - Kw, i = 1, 2. 

Thus the a posterior* cross-covariance matrix is 

EfreJ] = EUex-KiHxei-K^} 
x{ej — K 2 H 2 h 2 — K 2 v 2 }T\ 

= E[(I - KxHx)«x%(I - K 2 H 7 ) t 
-(I - KxHx)exvl Kl 
- Kxvx${l - K 7 H 2 ) t 
+K lViv^Ef], 

so that 

R = (/ - K { H X )R(I - K 2 H 2 f , (A. 10) 

where P has been propagated from the last update interval and £[ei vj], £[vi ej], and E[v x vj] have been 
assumed to be zero. Note that if the two filters were processing identical measurements, £[vi v^] = £[vi vf] = 
V\ and eq. A. 10 would become 

R = (/ - KiH x )R(I - K 2 H 2 ) T + KiVKj. 
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Bar-Shalom [5] has shown that R should be propagated in the same fashion as that used for each filter's i auto- 1 
covariance matrix, i.e.. via eq. A 3, under the assumption that both filters use the same covariance propagation 
method that accurately represents the propagation of the true state error covariance matrix. Also, because 
the measurements are assumed to be uncorrelated, the correlations modeled by R arise only due to the filters 
common system dynamics model. Therefore, R = 0 initially since no propagation has yet occurred Hence. 

R(t) =*(Uj)R(tj)* T (t.t } )+S(t)\ R(t o ) = 0. (All) 

We now choose an optimal W by minimizing the trace of P. , that is, 

minj = min trP.. 


The following properties of the trace operator are useful in the subsequent derivation: 

dtrAB T _ dtrBA T _ 

dB ~ 8B ~ ’ 

AnEAE— = 2 BA, if A is symmetric. 
dB 

Taking the partial derivative of J with respect to W yields 

= 2W(P l + P 2 -R-R T )- 2( A - R). 

oW 

The optimal W is then found by setting to zero, 


dJ 

dW 


w.„ 


= 0, 


and solving for W op x as follows: 

= (A - R)(Pi + P 2 - R — R T r l . 

A great deal of simplification for P, , given in eq. A. 8, result* if eq. A. 12 is used in eq. A. 8: 

P. = A - (A - R)(Pi + Pi-R- R T r T (Pi - R) t 
- (A - R){ A dr Pi - R- ft T ) _1 (A - R T ) 

+ (A - A + pi-r- R T r l 
x (A + A-fl- R t ) 
x (A + A - R - R t )~ t (Pi - Rf 


Taking advantage of cancellations, we have 

A = A - (A - A)(A + Pi- R- R t )~H A - fl r ), 


(A. 12) 


which reduces to 

A = A - Wop, {Pi - r t ). 


(A. 13) 


Hereafter, the algorithm specified by (in order) eqs. A.ll, A.10, A. 12, A.4, and A.13 will be referred to as a 
fusion filter. This algorithm is depicted in figure A. 2. 
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Figure A. 2. Algorithm for estimate fusion. 


Interpretation of the Cross- Covariance, R 

Under the assumption of common propagation models, the two Kalman filters have three statistically independent 
sources of information: measurements available to the first filter, measurements available to the second filter, and 
estimates propagated from the last measurement update [3]. While the propagated estimates are conditioned 
on different sets of measurements for the two filters, both use the same equations of motion and the same state 
noise intensity. This gives rise to correlations which are modeled by R. If these correlations are ignored, the 
fusion filter will underestimate its covariance matrix. Larger estimation errors, and possibly filter divergence, 
may result. 

A. 4. 2 Reducing Data Transmission Requirements 

Figure A, 3 shows the flow of information in the data fusion algorithm presented above; three matrices and a 
vector must be transmitted from each Kalman filter to the fusion filter whenever a measurement is processed. 
In this section, some methods for achieving reductions are discussed. 

Updating R using the A Priori Covariances 

As shown by eq. A. 10, to update the cross-covariance, the Kalman gain and measurement geometry matrices 
from the Kalman filters must be available to the fusion filter. However, recognizing that 

A = (/- 

it follows that 

PiP- 1 = (I - KiHi); i — 1,2, (A. 14) 

and an alternate form for the cross-covariance update is 

r = 

This update formula may be employed for situations in which the Kalman filters transmit estimates and covari- 
ances only. However, since two additional matrix inversions are required, speed and numerical accuracy may be 
compromised. Careful consideration must be given to the issue of whether or not these disadvantages offset the 
decrease in data transmission requirements for a particular application. 
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Figure A. 3. Information flow for estimate fusion. 


The Optimal Scalar Gain Approximation 

In some cases, even the covariances from the Kalman filters may not be readily available. An example is the 
typical Global Positioning System (GPS) receiver which often provides as output only a state estimate and a 
figure of merit. This figure of merit is typically derived from the trace of the GPS filter’s covariance matrix or 
some portion thereof. In this section, a formulation of the fusion filter will be given which utilizes a figure of 
merit based on the covariance traces. 

Following the derivation of the optimal fusion gain, assume a form for the fusion state update equation as 
follows: 

x„ = (1 - u>)xi + u>x 2 . 

Here w is restricted to be a scalar, and the subscript V indicates an optimal combination using the scalar gain. 
Now, rewriting this in terms of the estimation errors, the covariance matrix for the fused estimate is 

A = E[e„ ej] 

= E[{ei - u/ei + u;e3}{ei - u«i + uie2} r ] 

= (1 - 2w + w 2 )Pi + w 2 Pi + (tv — + R t ). 

A 

As before, the gain is determined by minimizing the coet function J = trP c where 

trP a = (1 - 2w + w 2 ) trP\ + w 2 trP 2 + 2(w - w 2 ) irk. 


Thus 


and 


= (-2 + 2 w) trP x + 2w trP 2 -h (2 - Aw) trR , 
aw 


d£ 

dw 


= 0 


implies that 

trPi — trR 
trP x + trP 2 — 2 trR 

An immediately apparent problem is the calculation of trR when A and P 2 are not available. However, 
since the use of a scalar gain will also introduce inaccuracies, simply neglecting the cross-covariance when using 
the scalar gain approximation may often be appropriate. An alternative may be to treat the trace of the cross- 
covariance as a tuning parameter whose size is chosen to force estimates calculated using the scalar gam to more 
accurately track estimates determined optimally. 
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Resetting a Kalman Filter via Estimate Fusion 

In some applications, it may be possible or desirable to use the fusion algorithm to reset the Kalman filters. With 
this method, the- state estimate and covariance matrix resulting from eqs. A. 4 and A. 13 are used to reinitialize 
both Kalman filters after each measurement has been processed at time t } . Thus 

x, = (/ - W opt )x { + W opt x 7y (A. 15) 

p, = P l -W opt (P l -R r y, i= 1,2. 

where indicates an estimate immediately posterior to a reset by the fusion algorithm. As a result, e! = e 2 . 
and 

R = E[e i e 2 ] 

= E[e i ei] = E[e 2 e 2 ] 

= Pi = P 2- (A. 16) 

However, it has been found that updating the cross-covariance as shown in eq. A. 16 may in some cases lead 
to numerical difficulties. This issue is currently under investigation. A (suboptimal) alternative to eq. A 16 is 
merely to let R = 0; then eq. A.ll becomes 

m = s(ty 

Hence, propagation of the cross-covariance is not required. Now, the fusion filter can be integrated with the 
Kalman filters by appending to the Kalman filters the additional update of eq. A. 15. Hereafter, a Kalman filter 
which is updated with estimates from another filter, via estimate fusion in the manner just described, will be 
referred to as an reinitialized Kalman filter (RKF). 


A. 5 Application to Lunar Rendezvous 


A. 5.1 Description 

Spacecraft rendezvous presents a scenario in which the estimate fusion algorithm might be beneficially applied. 
In this problem, accurate relative state and inertial state information are required for good performance, yet 
both are not typically available from the same navigation processor. In the problem considered here, an active 
spacecraft orbiting the Moon in a near-circular orbit is attempting to rendezvous with a passive spacecraft in a 
neighboring coplanar circular orbit. Initial uncertainties in the active vehicle's estimate of its own state corrupt 
its initial intercept maneuver. It is desired to perform a midcourse correction once an updated state estimate is 
available from the vehicle’s navigation system. 

This navigation system is a distributed system consisting of two Kalman filters. One filter, referred to as the 
rendezvous filter, processes discrete measurements derived from a radar system of range and elevation angle to 

the passive vehicle, pr } and &t, , viz., 

pr, = \fnri) - M«>)) T (rT(<j) - m*>)) + «*«■>,* 


o T) 


= arc tan 






where 

v (pt), ~ W(0, V [PT)i 6jk), v { , t)i ~ N(0, v (tr) 6 jk ), 

rc(t ; ) is the active vehicle (chaser) position, rr(* ; ) is the target vehicle position, and j = 1,2,.... This filter 
assumes perfect knowledge of the target states; hence, it applies its update only to the chaser states. 

The other -filter, referred to as the ground beacon filter, processes discrete measurements of the range from 
two beacons on the lunar surface, psx , and pb? } • The beacon positions lie on the vehicles’ common ground track 
and have been previously surveyed to high precision. These measurements are derived from the transit time of 
a signal broadcast by the beacon and are modeled as 


PBi, = >/(r a, - rc(<j)) T (r fll - r c (t,)) + v {pb ,) j 
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Figure A. 4. Rendezvous maneuver, inertial perspective. 


where i = 1,2 V( PBt ) ~ N(0, an< ^ i ~ ^2,.... 

Both filters model the spacecraft dynamics using a Keplerian gravity model which is augmented by Gaussian 
process noise. A somewhat different model, consisting of a Keplerian gravity model augmented by Gaussian 
process noise and a bias term, is used for the environment dynamics. Thus the dynamics are given by 

rc(0 = -(p + M„ “ Wr - 

and . v 

r T (<) = -(p + 6p) rTl ^ 3 ~ w f , 

IMOII 

where 6ft = 0 and w r ~ iV(0, W*) for the filter models, and w >- ~ N( 0, W?) for the environment. The filters 
are tuned by proper choice of W? to compensate for this imperfect knowledge of the gravity field. 

It is expected that the rendezvous filter will produce accurate estimates of the relative position and velocity 
between the two vehicles but inferior estimates of the inertial states of both vehicles. The occurrence of large 
inertial state errors could lead to inaccurate maneuver targeting solutions as well as a buildup of relative state 
errors during propagation intervals. To prevent the occurrence of such deleterious effects, it is desired to rectify 
both Kalman filters through the use of a fusion algorithm which optimally combines the estimates of the ren- 
dezvous and ground beacon filters. The optimal state estimate and error covariance will then be used to reset 
both filters, thereby converting the filters into two identical RKFs. (In applying the estimate fusion algorithm, 
filter 1 is taken to be the rendezvous filter, and filter 2 to be the ground beacon filter.) 

The passive vehicle’s orbit has a radius of two lunar radii. The active vehicle begins its maneuver 100 km 
behind and 50 km below the passive vehicle, as measured in a curvilinear target-fixed coordinate frame. The 
transfer is constrained to occur over a 30 degree arc, beginning at longitude 345 degrees and ending at longitude 
15 degrees. The ground beacons, located at longitudes 330 degrees and 30 degrees, remain visible during the 
entire maneuver. The selenographic frame to which the ground stations are fixed is assumed to be nonrotating, 
an approximation due to the short length of the maneuver. The orbit transfer takes approximately 25 minutes, 
and the midcourse correction maneuver occurs approximately halfway through the transfer near longitude 0 
degrees. The initial and midcourse burns are computed using Hill’s equations. The nominal maneuver is shown 
in figures A. 4 and A. 5, which depict the maneuver from inertial and target-fixed viewpoints, ^respectively. The 
design parameters used in tuning the filters are shown in table A.l. Note also that 6n = 10 *p and the RMS 
acceleration noise for the environment is 0 00 W||r c (to)lr where « s the initial time 


A. 5. 2 Results 

In figure A. 6, a typical performance case of the rendezvous filter and the ground beacon filter is shown. The solid 
lines represent estimation errors and the dashed lines the corresponding root mean square uncertainties of these 
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Figure A. 5. Rendezvous maneuver, target-fixed perspective. 


Table A.l. Filter Design Parameters 



Rendez- 

vous 

filter 

Ground 

beacon 

filter 

Init. RMS Pos. Err., m 

100 

100 

Init. RMS Vel. Err., m/sec a 

2 

2 

RMS Accel. Noise, m/sec 3 b 

0.1 

0.1 

RMS Range Mess. Err., m 

30 

30 

RMS Ang. Meas. Err., rad 

0.15 

- 

Meas. Interval, sec 

60 

60 


a Initial errors uncorrelated; applied equally in all channels 
b Noise assumed to be uncorrelatedt and equal in all channels 


errors as derived from the error covariance matrices. Shown here are radial and downtrack components of the 
errors in the estimates of chaser vehicle inertial position. This figure may be compared with figure A. 7, which 
shows the estimation errors for the same quantities as calculated by an optimal and a suboptimal RKF. The 
suboptimal RKF ignores the presence of correlations between the two Kalman filters. As indicated by the root 
mean square bounds on the estimation errors, aiding the Kalman filters using either of the fusion reset algorithms 
has brought significant performance improvements, most notably in the radial channel of the rendezvous filter 
during the first half of the maneuver. The fairly large estimation errors present after the last measurement in 
the downtrack channels of the RKFs are due to the presence of nonlinearities in the elevation angle measurement 
which were neglected in computing the H matrix. Incorporation of angular measurements at such close range is 
typically inhibited by filter designers. Note also that no noticeable degradation in accuracy occurs if correlations 
are ignored by the RKF. 

Relative state estimation errors, expressed in terms of coordinates in the direction parallel to the line-of-sight 
between the two vehicles and its normal direction, are shown in figure A.8. The rendezvous filter is compared 
to the optimal RKF. Since the target vehicle states are assumed to be perfectly known to these filters, trends 
similar to those noted for the inertial state estimates are observable here. In particular, it is clear that the 
stand-alone filter performs similarly to the RKF along the line-of-sight to the passive vehicle, but its accuracy 
normal to the line-of-sight is heavily range-dependent. The RKF, which also contains the data from the ground 
beacon filter, has enhanced ability to accurately estimate the relative state components which are normal to the 
line-of-sight. 

A great deal more insight into the process of estimate fusion may be gained through visualization of the 
optimal combination. To this end, projections onto the position plane of the hyperellipsoids corresponding to 
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Figure A. 10. l-<r error ellipses alter first measurement. 


the filters’ error covariance matrices are shown in figures A. 9 - A. 12. The most striking feature of these plots 
is the manner in which the ellipses corresponding to the RKFs are contained within the intersections of the 
ellipses corresponding to the stand-alone filters. These plots also show how the suboptim&l RKF, which ignores 
the presence of the cross-covariance, underestimates the size of the error ellipse corresponding to the optimal 
combination. 

Note also that the rotation of the ellipse deriving from the ground beacon filter, indicated by the dashed 
line, corresponds to the orbital motion of the chaser vehicle. The ellipse corresponding to the rendezvous filter 
maintains a fairly constant orientation since the line-of-sight between the two vehicles is fairly constant, but as 
the relative distance closes, the accuracy of the angular measurements in this filter exceeds the accuracy of its 
range measurements such that its semimajor and semiminor axes become transposed. Note also the close match 
in size and orientation of the ellipses corresponding to optimal and suboptimal RKFs. 

Use of The jScalar Gain Approximation 

Next, the effectiveness of the scalar gain approximation is evaluated. In this implementation, the actual cross- 
covariance is calculated, and its trace is used in computing the scalar gain. As shown in figure A. 13, the errors 
in inertial position estimates generated using the RKF with a scalar gain are generally of similar magnitude 
to those of the nominal RKF, albeit slightly noisier. Although the RMS error bounds are larger when a scalar 
gain is used, figure A. 14 shows that the RMS errors are still smaller for the RKF with scalar gain than for the 
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stand-alone rendezvous filter. (Since perfect knowledge of the target states is assumed, the inertial errors and 
relative errors represent the same quantities expressed in different reference frames.) 

Figures A. 15 - A. 18 indicate exactly how these error bounds behave in the position plane. The ellipse 
associated with the estimates fused using a scalar gain no longer lies wholly within the intersection of the two 
Kalman filters’ error ellipses. However, the RKF’s ellipse remains smaller than either of the ellipses associated 
with the standalone filters, indicating that even this approximate method of estimate fusion yields better results 
than either filter individually. 

The performance of the RKF using a scalar gain can also be contrasted to the performance of an RKF which 
ignored the cross-covariance. Recall that figures A.9 - A. 12 show that the suboptimal RKF underestimates the 
size of the covariance of the optimally combined estimate, whereas figures A. 15 - A. 18 show that the RKF which 
uses a scalar gain overestimates the size of this covariance. The scalar gain approximation, therefore, provides 
a more conservative estimate of its own accuracy than the optimal filter, whereas the RKF which ignores R is 
less conservative than the optimal filter. 


A.6 Conclusions 

A new derivation of an algorithm which fuses the outputs of two Kalman filters has been presented This 
algorithm has been shown to yield the optimal combination of the filters’ estimates, in the sense that the trace 
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of the covariance matrix of the optimal combination is minimized. Several modifications to the algorithm which 
reduce data transmission requirements have also been presented, including a scalar gain formulation, a cross- 
covariance update formula which employs only the two filters’ autocovariances, and a form of the algorithm 
which resets the two Kalman filters, leading to a modified Kalman filter, the reinitialized Kalman filter. 

This work has been applied to the problem of lunar rendezvous, in which one Kalman filter processes relative 
measurements and the other inertial measurements. Although some restrictive assumptions are made in the 
analysis, including perfect knowledge of the target vehicle’s state, promising results are obtained, demonstrating 
the effectiveness of the optimal RKF as well as forms of the RKF which ignore the cross-covariance matrix and 
use a scalar gain approximation. 

Plots of the position planes of the error hyperellipsoids corresponding to the various filters’ covariance ma- 
trices show that the ellipse corresponding to the covariance of the optimal combination is contained within the 
intersection of the two stand-alone Kalman filters’ error ellipses. The scalar gain approximation overestimates 
the size of this ellipse, while the RKF which ignores the cross-covariance underestimates the size. 

A potential problem with the approach taken here becomes evident if errors in the target vehicle states are 
introduced. The rendezvous filter will be able to accurately estimate the relative states but not the chaser s 
inertial states. Unfortunately, when the accurate inertial information from the ground beacon filter is fused with 
the accurate relative state information, degraded estimates of both result. This occurs because the target states 
are assumed to be perfect in the rendezvous filter, so the update resulting from the fusion algorithm is artificially 
constrained to be applied entirely to the chaser vehicle inertial states. If both the target and chaser states could 
be updated, then it seems likely that better results would be achieved. This would require the addition of target 
state estimation to the rendezvous filter, making its state vector incompatible for fusion with the ground beacon 
filter via the present method. This issue will be addressed in a forthcoming publication. 
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Abstract 

Techniques for optimally mixing the outputs from a pair of Kalman filters are presented, generalizing results 
previously presented. These techniques are derived under the assumption that the designs of the filters are 
fixed, and cannot be modified to support fusion requirements. A sufficient condition for using the optimally 
fused estimates to periodically reinitialize the Kalman filters is presented as well. The results are applied to an 
optimal spacecraft rendezvous problem, and simulated performance results indicate that use of the optimally 
fused data leads to significantly improved robustness to initial target vehicle state errors. Two other applications 
of estimate fusion methods to spacecraft rendezvous are also discussed: state vector differencing and redundancy 
management. 

B.l Introduction 

Historically, navigation systems have consisted of arrays of sensors which provided indirect or partial measure- 
ments of position, velocity, and attitude. In such systems, these measurements are passed in raw or minimally 
smoothed form to a centralized computing facility where they are typically processed by a statistical estimator 
such as a Kalman filter. With the advent of modern microprocessors, it has become increasingly possible to 
produce smart sensors, in which the state estimation process is moved inside the navigation sensor box. A 
typical example is the user segment of the Global Positioning System (GPS), in which the receiver and nav- 
igation software are usually integrated into a single receiver/ processor. Decentralizing the navigation process 
in this fashion has obvious advantages in terms of spreading the overall computational burden among parallel 
processors, and as a consequence, increasing fault tolerance at the cost of requiring a solution to a potentially 
complex integration problem. 

Although a solution to the problem of optimally fusing the outputs from two or more local estimators was 
presented at least as long ago as 1976 by Willner, et al. [1], this problem has received considerable attention 
in the literature of roughly the last fifteen years, typically with a focus on minimizing computation and/or 
communication requirements. One of the first of these recent works was the concise solution by Speyer [2] of 
the discrete and continuous decentralized Linear Quadratic Gaussian control problems. This work is notable 
for compressing all the information communicated between local processors into a data vector which has only 
the dimension of the control vector (if only the estimation problem is being solved, then the data vector has 
the same dimension as the state vector). Speyer's work was generalized in the works of Willsky, et al. [3]. 
These workers presented necessary and sufficient conditions for estimating a global state from local estimates 
of arbitrary dimension and expressed in arbitrary coordinate frames. Willsky, et al. predicted that their work 
could be simplified. One such simplification can be found in the work of Alouani and Birdwell [4]. These authors 
applied the results of their solution to the nonlinear estimation problem to the linear data fusion problem. In 
all of the approaches just cited, a great deal of the data transmitted between the local processors is related 
to correlations among the processors which arise due to common initial conditions and/or common process 
noise. One solution which eliminates some of these requirements (at least in comparison to Speyer’s work) is the 
unification collating filter which has been described by Kerr [22]. In this work, only the information needed to 
construct a globally optimal estimate at one, rather than all, of the local nodes is presented. Bierman [23] has 
presented an approach in which the cross-correlations are eliminated by constructing the local processing such 
that the information to be combined does not contain such correlations. More recently, Carlson has developed 
an approach known as federated filtering which extends Bierman’s approach [24]. Rather than assigning all of 
the common information to a single one of the local estimators, as in Bierman, Carlson constructs the local 
estimators such that the common information is disjointly shared. In contrast, the works of Bar-Shalom ([5], [6], 
[7]) have indicated how the cross-correlations can be advantageously used in the data association problem, for 
which it must be determined whether or not two estimates that are to be combined actually originate from the 
same tracked object. Also, the problem in which some data sources are raw measurements and others are the 
outputs of estimation schemes has been examined both in Willsky, et al. [3] and by Blackman [8]. In addition to 
these theoretical works, numerous interesting implementations of decentralized filtering architectures have been 
presented; the works of Wei and Schwarz [20] and Oshman and Isakow [25] are two recent examples. 

In one of our previous works [26], a solution to the problem of fusing two Kalman filters operating in parallel 
is presented in the context of spacecraft navigation. In the approach presented there, the outputs, or state 
estimates, of the two filters are combined using weights based on the filters’ covariance matrices as well as 
the cross-covariance accounting for any correlation between the filters. This approach was motivated by the 



B 2 PROBLEM STATEMENT 


no 


problem of retrofitting GPS onto the Space Shuttle because it was desired to avoid modifications to existing 
GPS and Space Shuttle navigation filters. To be a candidate solution for this problem, a data fusion algorithm 
must efficiently fuse the outputs of two local filters without requiring modifications inside the local filters, e g . 
by adjusting the local processors to eliminate cross-covariances. We called the approach taken estimate fusion 
to distinguish it from other solution methods to the data fusion problem. This paper extends the approach 
presented in our previous work by presenting the solution to the problem of fusing two filters with possibly 
noncommon states as well as to the problem of how the fused estimate and its covariance can be used to 
periodically reinitialize the Kalman filters and at what rate this reinitialization should take place. Results from 
application of the estimate fusion technique to a spacecraft rendezvous scenario are shown and the technique 
is found to combine in a complementary way the accuracies of a filter with relative state measurements and a 
filter with inertial state measurements. Additionally, two other examples of estimate fusions applicability to 
spacecraft rendezvous problems are described, state vector differencing and redundancy management. 


Notational Conventions 

In the sequel, scalars are denoted by lower case letters set in italic type, e.g., x or a; matrices are denoted by 
upper case letters set in italic type, e.g., A or f; and vectors are denoted by upper or lower case letters set in 
bold italic type, e.g., y, or X 1 . Random variables are denoted by letters set in sans serif type, e.g., x (a 
random vector) or h (a random scalar), and realizations of random variables are denoted as ordinary vectors 
and scalars. A normally distributed random variable r with mean /i and variance <r is denoted by r 
The Dirac delta function is denoted by 6(t - r) and the Kronecker delta by 6 jk . A posteriori estimates, i.e., 
estimates immediately following a measurement update, are denoted by the accent 4 and a pnon estimates, 
i.e., estimates immediately prior to the incorporation of a new measurement, are denoted by the accent 4 '\ 


B.2 Problem Statement 

Consider the case in which two continuous/discrete extended Kalman filters ([17], [19]) are operating on a system 
modeled by filter 1 as 

*i(<) = /i(*i(0) + w i( ( ); *i(0 ~ N(0,Si(t)6{t - r)), 

yi, = hi(*(tj)) + Vi;; Wij ~ N{0,RijS jk ), 

and modeled by filter 2 as 


x 2 (<) = / 2 (xj(0) + Wj (t); Wj(f) ~ N(Q,Si(t)6(t - r)), 


y 2; - = h 3 (x(tj)) + V 3 ;j vj> ~ W(0 ,R7j6jk), 

where filter 1 processes the discrete sequence of random measurements Y,(j) = {yn>yij,- .Yi,} anc * ^Her 

2 processes the measurements Y 3 (j) = {y 2l ,y 3j , . . • ,y aj }• We allow that the filters’ states and measurements 
may be divided into subsets common to both filters and subsets unique to each. Also, although not explicitly 
indicated in this work, the common subsets may be expressed in different coordinate frames, in which case the 
transformation between these frames must be appended to the algorithms shown here. (As noted by Willsky, 
et al. [3], there does not in fact have to be any physical relationship between the subsets viewed as common as 
long as any assumptions about relationships in the mapping of the states onto the measurements is preserved 
in both filters’ model realizations.) We assume the process noises w, (t) and *2(<). as well as the measurement 

noises v ly and v 2 ;, to be correlated where Si 2 (f)6(f - r) = E[wi(<) <*2 (f)]. and R\2j6jk = £[ v ij w 2t] However, 
we require only that both S 12 (<) and Ruj be nonnegative definite to allow that state or measurement subsets 
unique to one filter could be uncorrelated with subsets unique to the other. Hereafter, the time index subscript 
j will be suppressed as appropriate for clarity. 

Given a sequence ( j = 1,2,...) of observations, Y.(j), which are realizations of the random variables Y ,(j), 
each filter (i = 1.2) propagates its state between measurements via (t, <t < t ; + i) 

*<( 0 = /.(*,(<)). 
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Figure B.l. Schematic of estimate fusion. 


with x % (t } ), the estimate from its previous update as its initial condition. The filters propagate their covariances 
using 

Pi(t) = *i(t, tj)Pi(tj + S«(<). 


where 

and 

Here, 


and 


m i E[{*i(t) - i.(o}{x,(o - i 










*.(«) 


5 Ai (0 = f * i (t,T)Si{r)*T{t,T)dr. 

Each filter updates its state estimate and state error covariance matrix at time tj using 

= *<(<;) + Kij[Vij - hi(ii{tj))] 


where K tj is the Kalman gain for filter i at time t, , 


Kij = + «.->] 


1-1 


and 


Hijiii) 


dhj(xi(tj)) 
dx t (tj ) 




It is assumed that the filters are stable and operating optimally with respect to their own measurements. 
The problem at hand is to fuse the outputs of the two Kalman filters, as depicted in figure B.l, in an optimal 
fashion. 






B 3 PROBLEM SOLUTION 


B.3 Problem Solution 


Let X! and x 2l the state vectors of filters 1 and 2, be partitioned according to those states which are common to 
both filters and those states which are unique to each; 


Xl = [xf , 


<] T . 


X 2 


[*[. xJ) T , 


where x^ are the states common to both filters, x n are the states unique to filter 1, and x^ are the states unique 
to filter 2. 


B.3.1 The Optimal Combination 

A form for the optimal combination, in which the filters’ a posteriori estimates are linearly mixed, is assumed 
as follows: 

X. = Wifi » + W 2 i 2 , 

where the gain matrices, W,,i = 1,2, are to be determined, and the subscript '*’ denotes a quantity resulting 
from fusing the estimates. The gain matrices are to be chosen such that 8. is an unbiased, minimum variance 
estimator of the state of the system. 

Since Xi and x 2 are Kalman filter estimates, these quantities may be assumed to be expressible as 

*, = [/- K,tf,(X,)]x, + Ki[H,{fi)Xi + 

for i = 1,2. The a posteriori estimation error is defined as 

*= 1 , 2 ,*; 


it follows that 

e. = x. - ^(X! - *i) - W 2 (x 2 - «j). 

By assuming that £?[£i | Y*i] = 0 and that the filters are operating optimally, the expectation of the fused 
estimation error, conditioned on the measurements of the filters, is found to be 


Fp.KY^Vj)] = £[( 

1 

X M 

l 

- Wi 

’ H ' 

X. 


x < J 


L 7 


-w 2 [ )i(Vi,y 2 )]- 


Choosing and W 2 to be complementary as follows: 



■ (/- W') 0 ' 


' W( 0 ' 

w x = 

0 / 

, W 2 = 

0 0 


0 0 


0 / 


implies that E[i* \ (Y\,Y 2 )] = 0. 

Next, in pursuit of a minimum variance fusion of the estimates, the covariance of the fused estimate is found: 

P. = E[*. i* | (ViTj)] 

= W^P^Wj + W 2 P 2 Wj 

+ WiQ wj + W 2 Q T Wj, (B 1) 

where P t = £[Mf I Vi], = £[Mf I Y 3 ], and Q = £[M? I (Yr,Y 2 )}. The latter, <?, represents the 
cross-covariance of filter 1 and filter 2 and is updated via 

Q = (/ - h\Hi{xi))Q(I - K 2 H 2 (x 2 )) T + KiRi 2 K 2 , 

where Q has been propagated from the last update interval [26]. The issue of how Q is propagated will be visited 
in the sequel. Note that in general, Q is neither symmetric nor square. 
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Now. Pi, Pi. and Q are partitioned into blocks corresponding to common and unique states, 


Then 


A 

Q 


A« 

pT p 
r \(n rir W 

Qu Q« 

. Qni Qn< . 


Pi 


A« A cc 

A« A« 


WiPiWf 


(I - W()P IU (I - Wtf (/ - W ( )P l( , 0 ■ 
P^U-Wt) T Pirn 0 

0 0 0 


WiQ Wj = 


(I-W ( )Q<(W? 0(I-W ( )Q (( ' 
QmWj 0 <?„< 

0 0 0 


( B.2) 


( B .3 ) 


W 2 Q T Wf = {WiQ W?) T 

' W&fol - W( f W ( <% ( 0 • 

= 0 0 0, 
. Qla-Wi) T Q& 0. 


and 


W 7 P 2 W% = 

Substituting eqs. B.2 - B.5 into eq. B.l yields 


W { P ni Wj 0 W'P^ ■ 
0 0 0 
A«w? 0 A« . 


(B.4) 


(B.5) 


A<. 


P(n. 

Ac. 

A*». 

Ac. 


= (/ - w t ) A« (i - w t ) T 
+(/ - W ( ) Q„ wj 
+w ( Qj ( (I - w ( f + w t A« Wj, 
= A T C. = ( ; - W ()P^ + W(Qne 

= P&. = (I-W ( )Q (< + W ( PT (I 

= Pirn ' Ac* = P L — Q*i C 1 

= A<c> 


where 


P. = 


A<. An. Ac. 

p[, A 




Pi Pi Pi 


*<• 


*. ^C. J 


Interestingly, even though only thoee states common to both filters are fused, the correlations between these 
states and those which are unique to both filters are updated. 

To minimize the variance, an optimal W { is chosen to minimize the trace of P. Note that 


trP. = tr[(I - WO A« (/ - Wt) T 

+(/ - WO Q" Wj + W ( Qf ( (/ - W0 T 
+ W ( P 2 « Wj + P lnv + P,cc). 

, « ^ ^ ^ 
i.e., the off-diagonal blocks of P. do not contribute to trP.. Therefore, since = 0 and = 0, the 

problem of determining the optimal weighting matrix, W^ 09i , is equivalent to the problem in which the filters 



B.3. PROBLEM SOLUTION 


fi9 


have identical process models, which was solved in the authors’ previous work [26] The optimal gam. W\ jpt is 
determined by setting to zero, yielding, 

WUft = + P Ui “ Qtt ~ 

I'se of the optimal gain simplifies the expression for P^ #l viz., 


B.3.2 Propagation of the Cross-Covariance 

The cross-covariances explicitly contain the shared memory of the two filters, which originates from common 
initial conditions and/or common process noise models. The shared memory is maintained in the fusion algo 
rithrrTs propagation stage. For disjoint measurement sets, it cannot be created through the updates, but only 
modified. Although we assume that the initial conditions and process noise models associated with the states 
unique to one filter are uncorrelated with those of the other filter, we allow that states unique to a given filter 
may be correlated (through initial conditions or process noise models) with the states common to both filters, 
allowing for a significant degree of information sharing between the filters. 

As with the extended Kalman filter covariance matrices, propagation of the cross-covariances may be ex- 
pressed in the notation of a Riccati equation or via state transition matrices. Since the latter is typically viewed 
as computationally superior (cf. [27]), we report this form only. The derivation closely parallels that of the 
Kalman filter s covariance propagation (e.g. [14]), so we only sketch certain unique aspects. By definition, 

Q(tj) = ^(liJIa^i^KViU-ll.VaU-l))] 

= - *i(t>)}{xa(<>) -*a(*i)} T ]- (B.6) 

By expressing the continuous-time process models as equivalent discrete-time difference equations, eq. B.6 may 
be expanded and the expectation carried out so that we arrive at 

Q(t, ) = (tj , tj - X )Q(tj - 1 )*r (tj , tj _ , ) + S A ia(lj ), 


in which 


and 


Sai2(1) = / 

Jt,-y 

5ia(<) = £[( ) ("&(<) »£(«> ) 1- 


[t is assumed that £[wi„(t) w£(*) ] = 0, since these noise terms are applied to the states unique to each filter. 
Then 


S 13 (t) = 


Sl2«(0 5lJ(c(0 

Sl2i)*(0 0 




This matrix, like the filter process noise spectral density matrices 5, and 52, is determined as part of the 
navigation system tuning process. 


B.3.3 Reinitializing the Kalman Filters 

It is possible' to use the fused estimate and its covariance to periodically reinitialize the Kalman filters via a 
feedback configuration, as shown in figure B.2. In this procedure, the main jobs of the block labeled Fusion in 
figure B.2 are to propagate the cross-covariance matrix between measurement updates and to update it each 
time either of the filters performs an update. Then, at some frequency less than or equal to the slower filter's 
update frequency, a fusion of the filters’ state estimates and covariances is performed, with the filters restarted 
with the fused state and covariance as initial conditions. 
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Pm 


Figure B.2. Schematic of estimate fusion feedback. 


When such a reinitialization is performed, the cross-covariance must also be reinitialized. Denote quantities 
posterior to such a reinitialization with the accent 4 V Then, the estimate and covariance of filter t are 
and Pi = P m , and 

q = E[t t*j i (Vi.yj)] = £[«.«: I (yi.y 2 )] 

= p. = A = K 

For the case of common process models and common filter update rates, = $2 and S 12 = S\ = 52, so that 

Q = #i0#2 + 5*12 
= + 5* 1 — #zft^2 + 5*2 

= A = ft, 

i.e., no propagation of the cross-covariance is required. 

Care must be taken, however, to ensure that the filters' common states are statistically independent before 
reinitialization. To see this, consider the difference between the filters' state estimates, defined as 

3f = *lf - *2f » 

and the difference covariance defined as 

PdU = £[(*if “•af)(®if “®2f) T |(Vi,Y2)] 

= ftff + - Qff 

Just after a reinitialisation, = 0 and = 0. The filters must be allowed to operate long enough between 
reinitializations for to become invertible so that W^ opi = (Piff - Qff can computed. If P is not 
invertible, then there exists some a which has at least one non-zero component such that 

a T P^a = 0 => a T 3{ = 0 

i.e., the components of 3^ are linearly dependent [29]. In the section B.6, it is shown that a sufficient condition for 
to be invertible tuAen *Ae filters share a common process model is that a sufficient number of measurements 
(j = 1,2, denoted by ji, have been processed by at least one of the filters such that its observability 

grammian, given by 

m 

e m = £ <p(tj , t m ) T Hj R - 1 , t m ). 

; = 1 
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has full rank. Although may still be invertible if fewer measurements are processed, if measurements are 
processed between reinitializations, invertib ility of is guaranteed. Note that the appearance of this singu- 
larity is solely a consequence of reinitializing the filters with exactly the same initial conditions and therefore, 
it does not appear if the feedback scheme is not used. It has been suggested that maintaining the filters and 
cross-covariances in information form could possibly avoid this singularity. We concur that this is an interesting 
research topic, but for present purposes it would violate the condition of our approach that the existing sub-filters 
not be modified. 

B.3.4 Data Transmission Requirements 

The reader interested in a discussion of the computation and transmission requirements of the estimate fusion 
algorithm is referred to reference [26]. In brief, for the limited case in which the filters have common process 
models, complete state observability from a single update cycle, and do not process any common measurements, 
the estimate fusion feedback algorithm can be implemented in such a way as to only require transmission of 
states and covariances, as long the fusion processor has access to both prior and posterior covariance matrices. A 
promising alternative is the optimal scalar gain formulation derived in [26] in which the estimates are fused using 
a scalar weighting factor computed using only the traces of the covariances and cross- covariance. In fact as in 
the application considered in the sequel, for cases for which there are frequent and accurate measurements, the 
cross-covariance may often be suboptimaily ignored without significantly affecting performance. In such cases, 
a great deal of the computation and transmission requirements of the estimate fusion algorithm are relieved. 
In other cases, ad-hoc approaches to modeling the effect of the cross-covariance, such as that suggested by 
Blackman [8], may be employed successfully. 

B.4 Applications to Spacecraft Rendezvous 

B.4.1 Fusion of Inertial and Relative State Estimates 

The problem of lunar rendezvous was studied in reference [26]. It was shown that estimate fusion techniques could 
be used to improve the performance of a relative navigation filter by fusing its states with the state estimates 
from an inertial Kalman filter. However, due to the limitation of the estimate fusion algorithm presented in 
reference [26] to common state dimensionality, a perfect target assumption had to be made so that both filters 
only estimated the chaser vehicle states. It was mentioned that if significant target errors were present, degraded 
state estimation and possibly filter divergence could occur. With the new results presented in this appendix, 
this problem can now be addressed. 

A brief description of the scenario is presented. The reader is referred to reference [26] for details. The navi- 
gation system is a distributed system consisting of two Kalman filters. One filter, referred to as the rendezvous 
filter, processes discrete measurements derived from a radar system of range and elevation angle to the target 

vehicle, and &t } , viz., 


PT, = yfTAtj) - r c {tj )) T (*r(*>) - rc(<j)) + 


e T) 


= arc tan 


n.(tj) - fc.(tj) 


+ W (*T) J - 


w here 

v (pt), ~ N ( Q ’ V(PT)/;*)> v (#t), ~ V(# T )/i»). 

r c (t } ) is the active vehicle (chaser) position, r T (<;) is the target vehicle position, and j = 1,2 Note that 

updates from these measurements are used to estimate both the target and chaser vehicle inertial states. 

The other filter, referred to as the the ground beacon filter, processes discrete measurements of the range 
from two beacons on the lunar surface, p B \, and pbi, The beacon positions lie on the vehicles’ common ground 
track and have been previously surveyed to high precision. These measurements are derived from the transit 
time of a signal broadcast by the beacon and are modeled as 


PBx, = \j (»*Bi - )) r (»*Bt - r c (t J )) + 
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where i = I, 2, v (pg% ) — N( 0, V [f>Bt) 6jk), and j = 1, 2 

Both filters model the spacecraft aynamics using a Keplerian gravity model which is augmented by stochastic 
process noise. A somewhat different model is used for the environment dynamics which consists of a Keplerian 
gravity model, augmented by stochastic process noise and a bias term. Thus the dynamics are given by 

rc{t) = -(/■< + ^)rc{t)/\\rc(t)\\ 3 - Wr, 


and 

rr(t) = -(/i + (5/i)r T («)/||r T (t)|| 3 - u»r, 

where = 0 and u; r — .V(0, W r S(t - r)) for the filter models and Wf ~ N(Q, W r 6{t - r)) for the environment. 
The filters compensate for their imperfect knowledge of the gravity field by choosing Wr conservatively. 

The passive vehicles orbit has a radius of two lunar radii. The active vehicle begins its maneuver 100 km 
behind and 50 km below the passive vehicle, as measured in a curvilinear target-fixed coordinate frame. The 
transfer is constrained to occur over a 30 degree arc, beginning at longitude 345 degrees and ending at longitude 15 
degrees. The ground beacons are located at longitudes 330 degrees and 30 degrees and remain visible during the 
entire maneuver. The selenographic frame to which the ground stations are fixed is assumed to be nonrotating, 
an approximation due to the short length of the maneuver. The orbit transfer takes approximately 25 minutes, 
and the midcourse correction maneuver occurs approximately halfway through the transfer, near longitude 0 
degrees. The initial and midcourse bums are computed using Hill's equations. The nominal maneuver is shown 
in figure B.3, which depicts the maneuver from a target-fixed viewpoint. The design parameters used in tuning 
the filters are shown in table B.l. Note also that Sp = 10“ V and the RMS acceleration noise for the environment 

isO.oowilMMII 2 



An indication of the performance of stand-alone versions of the filters can be gleaned from figure B.4, which 
presents simulated data. The filters' performance in estimating the chaser vehicle inertial position states is shown 
The upper-left subplot shows the unaided ground beacon filter's estimation errors for the radial component of 
the chaser vehicle inertial position and the upper-right subplot shows this filter's performance for the downtrack 
component. Similarly, the two lower subplots show the unaided rendezvous filter’s estimation errors for the 
radial and downtrack component of the chaser vehicle’s inertial position, respectively. In this and subsequent 
plots, solid traces represent estimation errors and dashed traces represent the corresponding root mean square 
uncertainties of these errors as derived from the error covariance matrices. Note that the error covariance of the 
rendezvous filter grows quite large. 

When these same filters are reinitialized every other measurement pass using the estimate fusion feedback 
scheme, the large uncertainty in the chaser vehicle’s inertial position exhibited by the rendezvous filter is removed 
by the information provided by the ground beacon filter, as seen in figure B.5. Here the performance of an optimal 
configuration is shown in the two upper subplots, with errors in the radial and downtrack components of inertial 
position shown in the left and right subplots, respectively. In the two lower subplots, the performance of a 
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Table B.l. Filter Design Parameters 



Rendez- 

vous 

filter 

Ground 

beacon 

filter 

Init. RMS Pos. Err., m 

100 

100 

Init. RMS Vel. Err., m/sec Q 

2 

2 

RMS Accel. Noise, m/sec 2 b 

0.1 

0.1 

RMS Range Meas. Err., m 

30 

30 

RMS Ang. Meas. Err., deg 

0.15 

- 

Meas. Interval, sec 

60 

60 


a Initial errors uncorrelated; applied equally in all channels 
6 Noise assumed to be uncorrelated, and equal in all channels 



Figure B.4. Stand-alone Kalman filters’ estimation errors for inertial chaser vehicle position. 


suboptimal estimate fusion feedback scheme in which the correlations between the two filters, modeled by the 
cross-covariance matrix, are ignored by assuming that Q = 0. In these subplots as well, errors in the radial and 
downtrack components of the chaser vehicle inertial position are shown in the left and right subplots, respectively. 

Finally, figure B.6 shows the simulated relative state estimation performance of the reinitialized and unaided 
rendezvous filters in the upper and lower subplots, respectively. In this figure, relative position errors along the 
line-of-sight from the chaser to the target are shown on the left, while errors in relative position normal to the 
line-of-sight are shown on the right. From this figure we see that the reinitialized filter approaches the relative 
state accuracy of the unaided filter only on update cycles in which estimate fusion is not performed. Apparently, 
the uncertainties in the ground beacon filter’s state estimates marginally corrupt the relative state estimates, 
although with the benefit of substantially improving inertial state estimation performance, as seen from the 
comparison of figures B.4 and B.5. 

One of the benefits of having good inertial state estimates in a rendezvous scenario is demonstrated in the 
next sequence of plots in which initial errors having ten times the standard deviation expected by the filters were 
introduced into the target vehicle inertial states as a stress case. In figures B.7 and B.8, the inertial position 
performance of the unaided filters is compared to the two versions of the reinitialized filter (with and without 
cross-covariance modeling). As seen in figure B.7, the unaided rendezvous filter s performance in estimating the 
chaser vehicle inertial states for this case is poor. The arrangement of subplots in this figure is the same as that 
of figure B.4, with the errors of the unaided ground beacon filter on the top, those of the unaided rendezvous 
filter on the bottom, and radial and downtrack components of the chaser vehicle inertial position errors on the 
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Figure B.5. Reinitialized Kalman filters’ estimation errors for inertial chaser vehicle position. 



Figure B.6. Estimation errors for relative position. 


left and right, respectively. By contrast, the reinitialized rendezvous filter, which makes use of very accurate 
inertial state estimates from the ground beacon filter, is not significantly degraded by the stress case in its ability 
to estimate the chaser vehicle states, as shown by figure B.8. The arrangement of subplots in this figure is the 
same as that of figure B.5, with the errors of the optimal reinitialized rendezvous filter on the top, those of the 
suboptimal reinitialized rendezvous filter on the bottom, and radial and downtrack components of the chaser 
vehicle inertial position errors on the left and right, respectively. 

The relative state estimation errors of the reinitialized and stand-alone rendezvous filters for the stress case 
are shown in the top and bottom subplots of figure B.9, respectively. Here, as in figure B.6, relative position 
errors along the line-of-sight from the chaser to the target are shown on the left, while errors in relative position 
normal to the line-of-sight are shown on the right. As in the nominal case, it can be seen relative state estimates 
of the reinitialized rendezvous filter are marginally less accurate than those of the stand-alone rendezvous filter, 
but in light of the stand-alone filter’s poor inertial state estimation, the marginal improvement in relative state 
accuracy seems dubious. 

B.4.2 Relative Navigation By State Vector Differencing 

Another approach to the relative navigation problem presented above can be taken if both vehicles are equipped 
with filters providing inertial state estimates and no relative sensor is used. If in the lunar rendezvous example 
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Figure B.7. Stand-alone Kalman filters’ estimation errors for inertial chaser vehicle position in the presence of 
significant initial errors in target vehicle states. 


Figure B.8. Reinitialized Kalman filters’ estimation errors for inertial chaser vehicle position in the presence of 
significant initial errors in target vehicle states. 

presented above both vehicles are equipped with sensors which measure the range to the same ground beacons 
and this range data can be transmitted to a common location, it can be processed in a Kalman filter to estimate 
the relative state. Alternatively, if each vehicle were equipped with a ground beacon filter which allowed it 
to estimate its own inertial state, then these state estimates could be differenced to determine the relative 
state. This approach, known as state vector differencing, can be criticized for not providing any measure of the 
relative state uncertainty, or for providing inaccurate measures of the uncertainty. However, the estimate fusion 
techniques presented in this paper provide a means for addressing this issue. 

In this case, the two filters do not share any state vector elements in common; however, the process models 
for the two distinct states may in fact be identical, or at least quite similar to one another, since both vehicles 
see the same environment. Letting t ) denote the chaser vehicle inertial states and £ denote the target vehicle 
inertial states, the optimally fused state, 4. , is simply 



where xi is the state vector of a filter estimating only inertial states on board the chaser vehicle, and x 2 is 
the state vector of a filter estimating only inertial states on board the target vehicle. The optimally fused a 
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Figure B.9. Estimation errors for relative position in the presence of significant initial errors in target vehicle 
states. 


postertori state error covariance is simply 

p _ [ A QrK 

“ [ <£< * J ’ 

where Pi and P 2 are the a posteriori state error covariances of filters 1 and 2, respectively, and Q n ( is the a 
posteriori cross-covariance, given by 

$„<(<*) = (/ - K lk H llt )Q n <(**)(/ - K»H ik ) T 
+«Sai?<(1|t)! Qn({t<>) — 0- 

Note that due to the possibly similar or even identical process models for the unique state elements, S& n <; ^ 0. It 
is this information in the cross-covariance which is often ignored in other approaches to state vector differencing. 
The optimal estimate of the relative state is 

8*12 = = ^1 ^ 2 - 

The error covariance associated with the optimal relative state is 

Au = E[(i.i -•„)(*.! -4. 2 ) t | (Vi.Vj)] 

— Pi + Pi - Qri( - ■ 

This matrix is a correct measure of the relative state uncertainty for the state vector differencing problem. 

B.4.3 Fusion of Redundant Navigation Systems 

It is a long standing requirement that critical systems on spacecraft have redundant backups in case of failure. 
During nominal operations, both primary and redundant navigation sensors often are operated in parallel, 
presenting the problem of redundancy management. Various schemes for accomplishing this task have evolved, 
many of which, though simple and expedient, are suboptimal, such as averaging or midvalue selection. Data 
fusion techniques, though placing a greater computational burden on the navigation system, can avoid some 
potential problems of these suboptimal approaches. For cases in which it is not possible or cost-effective to 
modify the local processors, the estimate fusion technique presented in this work is a reasonable approach to 
redundancy management. 




B.5. ESTIMATE FUSION FEEDBACK OBSERVABILITY 


Since in this case the filters are identical, the estimate fusion algorithm as presented in reference [26] can 
be used directly. Note that the form of the cross-covariance update which includes the assumption of identical 
measurements, i.e.. 


<? = (/- A - A' 2 tf 2 (x 2 )) r + K,RKj 
= PxPCQPf'Pi + KiRKL 

must be employed, necessitating the transfer of the Kalman gain matrices, A'i and K 2 , in addition to the other 
parameters already required for the estimate fusion process. As long as the filters are operating identically, the 
estimate fusion provides estimates which are no better than any one of the filters operating alone. In fact, the 
filters will probably never operate identically and the estimate fusion algorithm will be able to optimally weight 
their outputs according to each filter s own estimate of its accuracy. If a sensor failure which is detected by 
the system’s filter occurs in one of the redundant systems so the filter stops incorporating new measurement 
information into its state and covariance matrix, then process noise in the filter’s model will tend to make the 
covariance grow. The estimates from this filter will then be downweighted by the estimate fusion in favor of 
the more accurate redundant filter. Note that in this application, reinitializing the Kalman filters with estimate 
fusion feedback is not appropriate. 

A significant problem with this approach to redundancy management is that very often the Kalman filter 
itself can fail, regardless of the health of the sensors, if its covariance gets too small, leading to divergence of 
the estimation errors when new information fails to be incorporated. This tendency is typically mitigated by 
appropriate tuning of the process and measurement noise intensities as well as by various forms of residual 
screening. In using residual screening filter designers are motivated by the conviction that by temporarily 
stopping the filter from incorporating new measurements its covariance matrix should begin to grow (due to 
the presence of additive process noise) and eventually become large enough to allow incorporation of new 
measurements once again, allowing the filter to return to stable operation. If this heuristic holds, then estimate 
fusion may initially result in degraded estimates while the divergent filter’s covariance is too small, but will 
eventually reflect the more accurate performance of the nominally operating filter. 

The estimate fusion technique can, however, provide an additional level of screening to protect against failures 
of the filters’ residual edit tests. An edit test at the state vector level can be performed as follows: compare the 
difference vector d\i = x\ - x 2 to the difference covariance Pdn = A + P7 - Q - Q T ; if the difference vector 
is smaller in some sense than the difference covariance (possibly multiplied by some factor), then allow x\ and 
x 2 to be fused; if not, then do not perform a fusion of the states. The way in which the difference vector is 
compared to the difference covariance is straightforward. To perform an n sigma edit test on the state vector 
difference between estimates from filters 1 and 2, calculate d i2 T Pj^dii and check to see whether it is less than 
or equal to n 2 . If it is, perform the data fusion. 

By monitoring the filters’ own residual screening results, it should be possible to determine which of the two 
filters has failed and exclude it from further consideration for data fusion. Alternatively, if there is a third source 
of information, such as a third filter, or a fused state which has been propagated since the last fusion interval, 
this state can be compared to x\ and *2 in the same way in order to determine which of the latter two should 
not be used. Further study of such issues is merited. 

B.5 Estimate Fusion Feedback Observability 

In this section, reference to common and noncommon elements of the filter state vectors, previously denoted by 
the subscripts £,rj, C> will be omitted for clarity. No loss of generality results from this omission since only the 
common elements are directly combined in the estimate fusion and, hence, relevant in the present context. Thus 
partial derivatives making up the matrices#, H, etc. should be understood to be taken with respect to only the 
common state vector elements. 

The situation being considered is that of two filters sharing a common process model, denoted by the 
subscripts 1 and 2, which have previously had their estimates and covariances fused and the fused estimate 
and covariance used to reinitialize the filters. As a result, the filters are maximally correlated. If a subsequent 
estimate fusion is to be performed, it has been shown that P d = P, + Pi-Q ~Q T must be inverted. Two useful 
facts will be required to investigate the problem of inverting this matrix. First, for a completely observable 
system with discrete measurements, j = 1,2 ,...,m, there exists a minimum number of measurements, fi. such 
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that for 

m 

Om = Tm) T Hj RJ ' H ,<P{t } , t m ) , 

; = i 

then m > n implies a T 0a > 0 for some a which has at least one non-zero component [30]. Next, the Kalman 
filter sequential covariance update (at time t m ), given by 


Pm = Pm - P m H l m {H m PmH £ + “ 1 , 


can be rewritten using the matrix inversion lemma: 


P m = (P- l + HlR^H m r l - 


Equivalently, a batch update formulation for the covariance in which a series of previously stored measurements 
are incorporated all at once at periodic intervals is as follows: 

A, = (P~ l + j2*jHj 

> = i 

where <Pj should be interpreted as the state transition matrix mapping from the time of the jth measurement 
to the (current) time at which the update is to be performed, i.e. 

The solution to the problem at hand is given by the following theorem. 

Theorem Either of the conditions m \ > pi or m? > /12 is sufficient for the existence of PJ l . 

Proof: Since the filters have common process models, if their covariances were propagated from the time of 
the previous estimate fusion without incorporating any new measurements until just prior to the next estimate 
fusion interval, then P\ = P2 = Q = P*. and the components of P* = Pi + ft - Q - Q T could be written in 
terms of P m using the matrix inversion lemma as follows: 

A = (A * 1 + EA r ^ fl r/^i;A )' 1 = (A“ l +© i) _l 

; = i 

ITlj 

Pi = (A * 1 + HlHZfHii*,)- 1 = (A - 1 +S1)- 1 

j=i 

Q = ( A * 1 + ©»r 1 A' l (A “ 1 + ©a ) - 1 - 


Thus it cam be seen that 


A = (A’ 1 + ©i)" l ©a(A’ 1 + ©i)“ l 

+(A _l + ©j) - 1 ©i(A ' 1 + ©i) _1 - 

Since the filters are assumed to be stable, (A -1 + ©•). * = 1,2 are full rank, positive definite matrices. Further, 
&\ or Si are at least positive semi-definite, so that the rank of A can be no less than the rank of the greater 
of (P.- 1 +0ir 1 ©3(A _l + ©s) _1 and (A* 1 +©5) _1 ©i(A"‘ + ©i)" 1 - Since (A -1 +©i).» = 1.2 are full rank, 
then from Sylvester’s inequality 

p[(Pr‘ +©ir‘©j(/ > r‘ +©j) _1 ] < m.n{p(P.-‘ +e,)-‘, 

p(©j),p(f > .- , +© a r 1 } 

= p(©j) 

p[(P. _1 + &i)~ l O\(P. 1 + 0i) -1 ] < mi»{p(Pf‘ 

p(eo,p(P.~ l +e i)- 1 } 

= p(Si) 

where p( ) represents ranifc( ). Hence, it can be seen that p(A) > m ^{p(S\), p(Si)}. If m i > Pi or m i > Pi- 
then Si or Si have full rank, respectively. Since Si, Si, and A share the same dimension, then m, > pi or 
mi > ixi is sufficient for A to have full rank, and hence be invertible. QED 
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